Teaching Assistant: Roi Yehoshua

Basic TAO plan example Runtime inspection of DM models World model (C)2015 Roi Yehoshua

TAO_PLAN(PLAN_NAME) { TAO_START_CONDITION TAO_ALLOCATION TAO_CALL_TASK(TaskName) TAO_STOP_CONDITION TAO_NEXT TAO_CLEANUP_BGN { TAO_CALL_TASK(TaskName) } TAO_CLEANUP_END } TAO_PLAN(PLAN_NAME) { TAO_START_CONDITION TAO_ALLOCATION TAO_CALL_TASK(TaskName) TAO_STOP_CONDITION TAO_NEXT TAO_CLEANUP_BGN { TAO_CALL_TASK(TaskName) } TAO_CLEANUP_END } Tasks called upon plan execution After STOP_CONDITION is satisfied, apply PROTOCOL to select next plan for execution Optional exit actions Allocate sub-plans Validated before plan is selected for running Validated all the time while the plan is running

(C)2015 Roi Yehoshua We will start with a basic plan that will increment a counter from 0 to 100 and will stop when reaching 100 Create a new package in dmw workspace called single_dm_demos with dependencies on decision_making and decision_making_parser: $ cd dmw/src $ catkin_create_pkg tao_plans roscpp decision_making decision_making_parser $ cd dmw/src $ catkin_create_pkg tao_plans roscpp decision_making decision_making_parser

(C)2015 Roi Yehoshua Compile the package and create an Eclipse project file for it: Now open the project in Eclipse If there are issues with compiling the project, then: – Go to Project Properties --> C/C++ General --> Preprocessor Include Paths, Macros, etc. --> Providers tab – Check CDT GCC Built-in Compiler Settings [Shared] – Rebuild the C/C++ index by Right click on project -> Index -> Rebuild $ cd ~/dmw $ catkin_make --force-cmake -G"Eclipse CDT4 - Unix Makefiles" $ cd ~/dmw $ catkin_make --force-cmake -G"Eclipse CDT4 - Unix Makefiles"

(C)2015 Roi Yehoshua Add a C++ source file to the project called BasicPlan.cpp

(C)2015 Roi Yehoshua #include #include using namespace std; using namespace decision_making; int counter = 0; #include #include using namespace std; using namespace decision_making; int counter = 0;

(C)2015 Roi Yehoshua TAO(Incrementer) { TAO_PLANS { Increment } TAO_START_PLAN(Increment); TAO_BGN { TAO_PLAN(Increment) { TAO_START_CONDITION(counter < 100); counter++; cout << "counter: " << counter << endl; sleep(1); TAO_ALLOCATE_EMPTY TAO_STOP_CONDITION(true); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(Increment); } TAO_END } TAO(Incrementer) { TAO_PLANS { Increment } TAO_START_PLAN(Increment); TAO_BGN { TAO_PLAN(Increment) { TAO_START_CONDITION(counter < 100); counter++; cout << "counter: " << counter << endl; sleep(1); TAO_ALLOCATE_EMPTY TAO_STOP_CONDITION(true); TAO_NEXT(NextFirstReady) { TAO_NEXT_PLAN(Increment); } TAO_END } The code block here is executed before the stop condition is checked. Thus, the stop condition is relevant only when working with tasks

(C)2015 Roi Yehoshua int main(int argc, char **argv) { ros::init(argc, argv, "basic_plan"); ros::NodeHandle nh; ros_decision_making_init(argc, argv); ros::AsyncSpinner spinner(1); spinner.start(); RosEventQueue eventQueue; CallContext context; eventQueue.async_spin(); ROS_INFO("Starting incrementer plan..."); TaoIncrementer(&context, &eventQueue); eventQueue.close(); ROS_INFO("TAO finished."); return 0; } int main(int argc, char **argv) { ros::init(argc, argv, "basic_plan"); ros::NodeHandle nh; ros_decision_making_init(argc, argv); ros::AsyncSpinner spinner(1); spinner.start(); RosEventQueue eventQueue; CallContext context; eventQueue.async_spin(); ROS_INFO("Starting incrementer plan..."); TaoIncrementer(&context, &eventQueue); eventQueue.close(); ROS_INFO("TAO finished."); return 0; }

(C)2015 Roi Yehoshua To execute a given TAO, call the following function in your C++ code: – ctx: pointer to current context Can be NULL – eventQueue: pointer to events system Cannot be NULL This function is blocked until TAO is finished (preempted, stopped by TAO_RESULT or EventQueue is closed) TaskResult TaoNAME(const CallContext* ctx, EventQueue* eventQueue)

(C)2015 Roi Yehoshua This class is used for sharing parameters and context information across different TAO machines Utility functions for handling parameters: – void createParameters(A* a= new A()) - create instance of parameters of type A(template) – A& parameters() - get parameters – bool isParametersDefined() const - check if parameters have been created before Context is used for teamwork as well (more on this later)

Add the following lines to CMakeLists.txt Build the package by calling catkin_make (C)2015 Roi Yehoshua add_executable(basic_plan src/BasicPlan.cpp) target_link_libraries(basic_plan ${catkin_LIBRARIES}) add_executable(basic_plan src/BasicPlan.cpp) target_link_libraries(basic_plan ${catkin_LIBRARIES})

Use rosrun to run the node: (C)2015 Roi Yehoshua

You can use ros and rqt to view, monitor, record and interact with the decision making model To enable visualization of the model add the following line to the CMakeLists.txt file (just after target_link_libraries command): This command ensures that your model will be converted to xml and dot formats (for visualization) decision_making_parsing(src/BasicPlan.cpp)

(C)2015 Roi Yehoshua The xml and dot files will be created in the folder ~/dmw/devel/share/tao_plans/graphs

(C)2015 Roi Yehoshua Incrementer plan’s graph (incrementer.dot.gif):

(C)2015 Roi Yehoshua Plan converted to XML:

(C)2015 Roi Yehoshua In order to see visually the model in runtime, use the Decision Making rqt plugin First run rqt: Choose the Decision Making graph plugin: $ rqt

Once the model is running, the visualization of your model should be loaded automatically (C)2015 Roi Yehoshua

WorldModel is a struct derived from CallContextParameters and holds parameters that are shared across TAO machines It must implement a str() function that returns its string representation In the next example we will use a world model to store the counter instead of the global counter variable Copy BasicPlan.cpp to BasicPlanWithWM.cpp and add the following code

(C)2015 Roi Yehoshua struct WorldModel : public CallContextParameters { int counter; string str() const { stringstream s; s << "counter=" << counter; return s.str(); } }; #define WM TAO_CONTEXT.parameters () struct WorldModel : public CallContextParameters { int counter; string str() const { stringstream s; s << "counter=" << counter; return s.str(); } }; #define WM TAO_CONTEXT.parameters ()

(C)2015 Roi Yehoshua TAO(Incrementer) { TAO_PLANS { Increment } TAO_START_PLAN(Increment); TAO_BGN { TAO_PLAN(Increment) { TAO_START_CONDITION(WM.counter < 100); WM.counter++; cout << "counter: " << WM.counter << endl; boost::this_thread::sleep(boost::posix_time::milliseconds(100)); TAO_ALLOCATE_EMPTY TAO_STOP_CONDITION(true); TAO_NEXT(NextFirstReady){ TAO_NEXT_PLAN(Increment); } TAO_END } TAO(Incrementer) { TAO_PLANS { Increment } TAO_START_PLAN(Increment); TAO_BGN { TAO_PLAN(Increment) { TAO_START_CONDITION(WM.counter < 100); WM.counter++; cout << "counter: " << WM.counter << endl; boost::this_thread::sleep(boost::posix_time::milliseconds(100)); TAO_ALLOCATE_EMPTY TAO_STOP_CONDITION(true); TAO_NEXT(NextFirstReady){ TAO_NEXT_PLAN(Increment); } TAO_END }

(C)2015 Roi Yehoshua int main(int argc, char **argv) { ros::init(argc, argv, "basic_plan_wm"); ros::NodeHandle nh; ros_decision_making_init(argc, argv); ros::AsyncSpinner spinner(1); spinner.start(); RosEventQueue eventQueue; CallContext context; context.createParameters(new WorldModel()); eventQueue.async_spin(); ROS_INFO("Starting incrementer plan..."); TaoIncrementer(&context, &eventQueue); eventQueue.close(); ROS_INFO("TAO finished."); return 0; } int main(int argc, char **argv) { ros::init(argc, argv, "basic_plan_wm"); ros::NodeHandle nh; ros_decision_making_init(argc, argv); ros::AsyncSpinner spinner(1); spinner.start(); RosEventQueue eventQueue; CallContext context; context.createParameters(new WorldModel()); eventQueue.async_spin(); ROS_INFO("Starting incrementer plan..."); TaoIncrementer(&context, &eventQueue); eventQueue.close(); ROS_INFO("TAO finished."); return 0; }

Use rosrun to run the node: (C)2015 Roi Yehoshua