TaoIncrement.cpp
Go to the documentation of this file.
00001 /*
00002  * Filename: TaoExample.cpp
00003  *   Author: Igor Makhtes
00004  *     Date: Dec 26, 2013
00005  *
00006  * The MIT License (MIT)
00007  *
00008  * Copyright (c) 2013 Cogniteam Ltd.
00009  *
00010 7 * Permission is hereby granted, free of charge, to any person obtaining a copy
00011  * of this software and associated documentation files (the "Software"), to deal
00012  * in the Software without restriction, including without limitation the rights
00013  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00014  * copies of the Software, and to permit persons to whom the Software is
00015  * furnished to do so, subject to the following conditions:
00016  *
00017  * The above copyright notice and this permission notice shall be included in
00018  * all copies or substantial portions of the Software.
00019  *
00020  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00021  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00022  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00023  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00024  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00025  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00026  * THE SOFTWARE.
00027  */
00028 
00029 #include <iostream>
00030 #include <boost/bind.hpp>
00031 #include <boost/thread.hpp>
00032 #include <boost/foreach.hpp>
00033 #include <ros/ros.h>
00034 
00035 #include <decision_making/TAO.h>
00036 #include <decision_making/TAOStdProtocols.h>
00037 #include <decision_making/ROSTask.h>
00038 #include <decision_making/DecisionMaking.h>
00039 
00040 using namespace std;
00041 using namespace decision_making;
00042 
00043 #define foreach BOOST_FOREACH
00044 
00045 TAO_HEADER(Even)
00046 TAO_HEADER(Odd)
00047 
00048 //#define TAO_STOP_CONDITION(X) TAO_STOP_CONDITION_AND_PRINT_EVENTS(X)
00049 
00050 struct WorldModel: public CallContextParameters{
00051         int i;
00052         string str()const{ stringstream s; s<<"i="<<i; return s.str(); }
00053 };
00054 #define WM call_ctx.parameters<WorldModel>()
00055 
00056 TAO(Incrementer)
00057 {
00058     TAO_PLANS{
00059         Increment,
00060     }
00061     TAO_START_PLAN(Increment);
00062     TAO_BGN{
00063         TAO_PLAN( Increment ){
00064             TAO_START_CONDITION( WM.i < 100 );
00065             TAO_ALLOCATE(AllocFirstReady){
00066                 TAO_SUBPLAN(Even);
00067                 TAO_SUBPLAN(Odd);
00068             }
00069             TAO_STOP_CONDITION( false );
00070             TAO_NEXT(NextFirstReady){
00071                 TAO_NEXT_PLAN(Increment);
00072             }
00073         }
00074     }
00075     TAO_END
00076 }
00077 
00078 TAO(Even)
00079 {
00080     TAO_PLANS{
00081         Even,
00082     }
00083     TAO_START_PLAN(Even);
00084     TAO_BGN
00085     {
00086         TAO_PLAN( Even ){
00087             TAO_START_CONDITION( WM.i%2 == 0 );
00088             WM.i++;
00089             cout<<"Even : "<<WM.str()<<endl;
00090             sleep(1);
00091             TAO_ALLOCATE_EMPTY
00092             TAO_STOP_CONDITION(true);
00093             TAO_NEXT_EMPTY
00094         }
00095     }
00096     TAO_END
00097 }
00098 
00099 TAO(Odd)
00100 {
00101     TAO_PLANS{
00102         Odd,
00103     }
00104     TAO_START_PLAN(Odd);
00105     TAO_BGN
00106     {
00107         TAO_PLAN( Odd ){
00108             TAO_START_CONDITION( WM.i%2 != 0 );
00109             WM.i++;
00110             cout<<"Odd : "<<WM.str()<<endl;
00111             sleep(1);
00112             TAO_ALLOCATE_EMPTY
00113             TAO_STOP_CONDITION(true);
00114             TAO_NEXT_EMPTY
00115         }
00116     }
00117     TAO_END
00118 }
00119 
00120 
00121 decision_making::TaskResult testTask(string name, const FSMCallContext& context, EventQueue& eventQueue) {
00122         cout<<"[testTask from "<<context.str()<<"]"<<endl;
00123     sleep(1);
00124     return TaskResult::SUCCESS();
00125 }
00126 
00127 int main(int argc, char **argv) {
00128 
00129     ros::init(argc, argv, "tao_example_incrementer");
00130 
00131     ROS_INFO("Starting...");
00132 
00133     ros_decision_making_init(argc, argv);
00134     ros::NodeHandle nodeHandle;
00135     RosEventQueue eventQueue;
00136 
00137     ros::AsyncSpinner spinner(1);
00138     spinner.start();
00139 
00140     LocalTasks::registrate("testTask", testTask);
00141 
00142     eventQueue.async_spin();
00143     CallContext call_ctx;
00144     call_ctx.createParameters(new WorldModel());
00145     TaoIncrementer(&call_ctx, &eventQueue);
00146     eventQueue.close();
00147     ROS_INFO("Finished.");
00148         return 0;
00149 }


decision_making_examples
Author(s):
autogenerated on Wed Aug 26 2015 11:17:01