1. Teaching Assistant: Roi Yehoshua roiyeho@gmail.com

2. Decision making in ROS TAO plans World model Tasks Runtime inspection of DM models (C)2014 Roi Yehoshua

3. http://wiki.ros.org/decision_making The goal of this package is to implement light- weight, generic and extendable tools for writing, executing, debugging and monitoring decision making models through ROS standard tools Decision making package is being actively developed by Cogniteam Cogniteam (C)2014 Roi Yehoshua

4. Copy dm_install.sh to your home directory – This shell script takes care of the DM package installation Copy also the relevant decision making tar file according to your operating system version The tar files are available on the course web site http://u.cs.biu.ac.il/~yehoshr1/89-689/ (C)2014 Roi Yehoshua

5. Add execution permission to the shell script Now run the following command: For example, if you have a 64-bit Ubuntu run: – The script will create the workspace directory dmw – It also adds appropriate 'source setup.bash' to the bashrc (C)2014 Roi Yehoshua $ chmod +x dm_install.sh $./dm_install.sh /path/to/workspace tar_file_name.tar $./dm_install.sh ~/dmw decision_making_13_04_x64_hydro.tar

6. If you have changed the ROS_PACKAGE_PATH variable in.bashrc, make sure its definition is at the end of the file: (C)2014 Roi Yehoshua # ROS setup source /opt/ros/hydro/setup.bash source ~/catkin_ws/devel/setup.bash source /home/roiyeho/dmw/install/setup.bash source /home/roiyeho/dmw/devel/setup.bash export ROS_PACKAGE_PATH=~/ros/stacks:${ROS_PACKAGE_PATH} export EDITOR='gedit' # ROS setup source /opt/ros/hydro/setup.bash source ~/catkin_ws/devel/setup.bash source /home/roiyeho/dmw/install/setup.bash source /home/roiyeho/dmw/devel/setup.bash export ROS_PACKAGE_PATH=~/ros/stacks:${ROS_PACKAGE_PATH} export EDITOR='gedit'

7. Under the ~/dmw/src you will find two folders with code examples – decision_making_examples – for single robot – dm_teamwork_examples – for multi robots You can launch any of the examples using the standard roslaunch command For example, to launch the Wandering Robot FSM example, type: (C)2014 Roi Yehoshua $ roslaunch decision_making_examples fsm_wandering.launch

8. (C)2014 Roi Yehoshua

9. Once the model is running, its visualization is displayed using the Decision Making rqt plugin (C)2014 Roi Yehoshua

10. Teamwork examples include the following: (C)2014 Roi Yehoshua

11. For example, to launch the formation example type: (C)2014 Roi Yehoshua $ roslaunch dm_teamwork_examples formation.launch

12. The decision making system supports different types of models: – FSM – Finite State Machines FSM – HSM – Hierarchical FSM HSM – Behavior Trees Behavior Trees – CogniTAO – implementation of BDI architecture CogniTAO In this course we will focus on CogniTAO (C)2014 Roi Yehoshua

13. CogniTAO (Think As One) is an implementation of the BDI architecture for both single robot missions and for multiple robots working in teams CogniTAO Main features: – Simulate entities that can execute complex missions in dynamic environments, where it is impossible to foresee all possible decisions – Coupling between the decision-making and the world modeling components – Mixing goal-oriented and reactive control, according to the principals of BDI (C)2014 Roi Yehoshua

14. Defines the current Task to be performed, contains Start and Stop conditions, and is coupled with corresponding Plans through Allocation and Next protocols

15. (C)2014 Roi Yehoshua A TAO is a level of a number of Plans and their corresponding "Sons", "Allocations", and "Next" protocols A deeper level of sub-plans creates another TAO Each TAO defines its starting plan via the TAO_START_PLAN tag

16. (C)2014 Roi Yehoshua Boolean conditions defined inside each "Task” Start conditions are validated before the plan is selected for running Stop conditions are validated throughout the entire running time of the plan The start and stop conditions are usually based on the world model (more on this later)

17. (C)2014 Roi Yehoshua The Next Protocol takes place when a current plan ends, and essentially chooses one next plan to be performed It has the possibility to loop its own plan (i.e. 'return' back and re-run the plan) Built-in Next protocols: – NextFirstReady You can also create your own Next protocols

18. (C)2014 Roi Yehoshua The Allocation Protocol takes place in a running plan, and essentially chooses (or divides) a sub-plan that will be performed It does not have the possibility to loop the given plan node If all children of an Allocation Protocol 'die' (i.e. there is no additional child that continues to its own Next protocol) then the father 'dies' Built-in Allocation protocols: – AllocFirstReady

19. (C)2014 Roi Yehoshua In teams, all members of the team correspond to the same Next protocol, while Allocation divides the team into sub-groups that correspond to the same Next protocols respectively

20. (C)2014 Roi Yehoshua TAOs are defined in a.cpp file using the following syntax: TAO(TAO_NAME) { TAO_PLANS { PLAN_1, PLAN_2,... } TAO_START_PLAN(PLAN_NAME); TAO_BGN { PLANS } TAO_END } TAO(TAO_NAME) { TAO_PLANS { PLAN_1, PLAN_2,... } TAO_START_PLAN(PLAN_NAME); TAO_BGN { PLANS } TAO_END }

21. (C)2014 Roi Yehoshua To reference another TAO before defining it, use the following declaration: TAO_HEADER(TAO_NAME);

22. (C)2014 Roi Yehoshua TAO_PLAN(PLAN_NAME) { TAO_START_CONDITION TAO_ALLOCATION TAO_CALL_TASK(TaskName) TAO_CLEANUP_BGN { TAO_CALL_TASK(TaskName) } TAO_CLEANUP_END TAO_STOP_CONDITION TAO_NEXT } TAO_PLAN(PLAN_NAME) { TAO_START_CONDITION TAO_ALLOCATION TAO_CALL_TASK(TaskName) TAO_CLEANUP_BGN { TAO_CALL_TASK(TaskName) } TAO_CLEANUP_END TAO_STOP_CONDITION TAO_NEXT } Must be located inside a TAO_BGN-TAO_END block All task calls after TAO_ALLOCATION section run in parallel as the plan begins 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

23. (C)2014 Roi Yehoshua TAO_ALLOCATE syntax: Each sub-plan is a start plan of a selected TAO If there are no sub-plans to allocate, then use the tag TAO_ALLOCATE_EMPTY TAO_ALLOCATE(PROTOCOL) { TAO_SUBPLAN(TAO_1), TAO_SUBPLAN(TAO_2),... } TAO_ALLOCATE(PROTOCOL) { TAO_SUBPLAN(TAO_1), TAO_SUBPLAN(TAO_2),... }

24. (C)2014 Roi Yehoshua TAO_NEXT syntax: If there are no next plans to execute, then use the tag TAO_NEXT_EMPTY TAO_NEXT(PROTOCOL) { TAO_NEXT_PLAN(PLAN_1), TAO_NEXT_PLAN(PLAN_2),... } TAO_NEXT(PROTOCOL) { TAO_NEXT_PLAN(PLAN_1), TAO_NEXT_PLAN(PLAN_2),... }

25. (C)2014 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

26. (C)2014 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"

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

28. (C)2014 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;

29. (C)2014 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

30. (C)2014 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; // CallContext must define a team teamwork::SharedMemory db; teamwork::Teammates teammates; teamwork::Team main_team = teamwork::createMainTeam(db, "main", teammates); context.team(TAO_CURRENT_TEAM_NAME, main_team.ptr()); 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; // CallContext must define a team teamwork::SharedMemory db; teamwork::Teammates teammates; teamwork::Team main_team = teamwork::createMainTeam(db, "main", teammates); context.team(TAO_CURRENT_TEAM_NAME, main_team.ptr()); eventQueue.async_spin(); ROS_INFO("Starting incrementer plan..."); TaoIncrementer(&context, &eventQueue); eventQueue.close(); ROS_INFO("TAO finished."); return 0; }

31. (C)2014 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)

32. (C)2014 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)

33. Add the following lines to CMakeLists.txt Build the package by calling catkin_make (C)2013 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})

34. Use rosrun to run the node: (C)2013 Roi Yehoshua

35. (C)2014 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)

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

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

38. (C)2014 Roi Yehoshua Plan converted to XML:

39. (C)2014 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

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

41. 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

42. (C)2014 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 ()

43. (C)2014 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 }

44. (C)2014 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()); teamwork::SharedMemory db; teamwork::Teammates teammates; teamwork::Team main_team = teamwork::createMainTeam(db, "main", teammates); context.team(TAO_CURRENT_TEAM_NAME, main_team.ptr()); 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()); teamwork::SharedMemory db; teamwork::Teammates teammates; teamwork::Team main_team = teamwork::createMainTeam(db, "main", teammates); context.team(TAO_CURRENT_TEAM_NAME, main_team.ptr()); eventQueue.async_spin(); ROS_INFO("Starting incrementer plan..."); TaoIncrementer(&context, &eventQueue); eventQueue.close(); ROS_INFO("TAO finished."); return 0; }

45. Use rosrun to run the node: (C)2013 Roi Yehoshua

46. (C)2014 Roi Yehoshua Task is an atomic preemptable action Each task is executed on a separate thread Decision making supports two types of Tasks: – Local task – a callback function – ROS remote task – a special actionlib client To create this kind of task, you need to extend RobotTask class from robot_task package Use tasks to implement asynchronous operations in your plan

47. (C)2014 Roi Yehoshua You need to register a local task before usage (otherwise, the system assumes it is remote) The callback function has the following signature: Task invocation: LocalTasks::registrate(''TASK_NAME'', callback); TaskResult FUNCTION_NAME(string task_name, const CallContext& context, EventQueue& eventQueue) TAO_CALL_TASK(task);

48. (C)2014 Roi Yehoshua Typically a long running task needs to check if it was preempted by an external event: TaskResult Task(...., EventQueue& eventQueue) { while(!eventQueue.isTerminated()){ PAUSE(250); } return TaskResult::SUCCESS(); } TaskResult Task(...., EventQueue& eventQueue) { while(!eventQueue.isTerminated()){ PAUSE(250); } return TaskResult::SUCCESS(); }

49. (C)2014 Roi Yehoshua For demonstration, we will turn the increment counter operation into a task Copy BasicPlan.cpp to BasicPlanWithTasks.cpp Make the following changes to the TAO machine definition

50. (C)2014 Roi Yehoshua TAO(Incrementer) { TAO_PLANS { Increment } TAO_START_PLAN(Increment); TAO_BGN { TAO_PLAN(Increment) { TAO_START_CONDITION(WM.counter < 100); TAO_CALL_TASK(incrementTask); 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); TAO_CALL_TASK(incrementTask); TAO_ALLOCATE_EMPTY TAO_STOP_CONDITION(true); TAO_NEXT(NextFirstReady){ TAO_NEXT_PLAN(Increment); } TAO_END }

51. Task callback function: (C)2014 Roi Yehoshua TaskResult incrementTask(string name, const CallContext& context, EventQueue& eventQueue) { cout << "[incrementTask from " << context.str() << "]" << endl; context.parameters ().counter++; cout ().counter << endl; boost::this_thread::sleep(boost::posix_time::milliseconds(100)); return TaskResult::SUCCESS(); } TaskResult incrementTask(string name, const CallContext& context, EventQueue& eventQueue) { cout << "[incrementTask from " << context.str() << "]" << endl; context.parameters ().counter++; cout ().counter << endl; boost::this_thread::sleep(boost::posix_time::milliseconds(100)); return TaskResult::SUCCESS(); }

52. Task registration: (C)2014 Roi Yehoshua int main(int argc, char **argv) { ros::init(argc, argv, "basic_plan_tasks"); ros::NodeHandle nh; ros_decision_making_init(argc, argv); ros::AsyncSpinner spinner(1); spinner.start(); LocalTasks::registrate("incrementTask", incrementTask); RosEventQueue eventQueue; CallContext context; context.createParameters(new WorldModel()); // CallContext must define a team … 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_tasks"); ros::NodeHandle nh; ros_decision_making_init(argc, argv); ros::AsyncSpinner spinner(1); spinner.start(); LocalTasks::registrate("incrementTask", incrementTask); RosEventQueue eventQueue; CallContext context; context.createParameters(new WorldModel()); // CallContext must define a team … eventQueue.async_spin(); ROS_INFO("Starting incrementer plan..."); TaoIncrementer(&context, &eventQueue); eventQueue.close(); ROS_INFO("TAO finished."); return 0; }

53. (C)2013 Roi Yehoshua

54. (C)2014 Roi Yehoshua When a stop condition becomes true while a task is running, a system_stop_event flag will be signaled However, the TAO machine will keep running until all the tasks are finished Thus, the task needs to periodically check if the event queue is terminated In the next example, the incrementer task will increment the counter to 200 and the stop condition will make the plan terminate when the counter reaches 100

55. (C)2014 Roi Yehoshua TAO(Incrementer) { TAO_PLANS { Increment } TAO_START_PLAN(Increment); TAO_BGN { TAO_PLAN(Increment) { TAO_START_CONDITION(true); TAO_CALL_TASK(incrementTask); TAO_ALLOCATE_EMPTY TAO_STOP_CONDITION(WM.counter == 100); TAO_NEXT_EMPTY } TAO_END } TAO(Incrementer) { TAO_PLANS { Increment } TAO_START_PLAN(Increment); TAO_BGN { TAO_PLAN(Increment) { TAO_START_CONDITION(true); TAO_CALL_TASK(incrementTask); TAO_ALLOCATE_EMPTY TAO_STOP_CONDITION(WM.counter == 100); TAO_NEXT_EMPTY } TAO_END }

56. (C)2014 Roi Yehoshua TaskResult incrementTask(string name, const CallContext& context, EventQueue& eventQueue) { cout << "[incrementTask from " << context.str() << "]" << endl; for (int i = 0; i < 200 && !eventQueue.isTerminated(); i++) { context.parameters ().counter++; cout ().counter << endl; boost::this_thread::sleep(boost::posix_time::milliseconds(100)); } return TaskResult::SUCCESS(); } TaskResult incrementTask(string name, const CallContext& context, EventQueue& eventQueue) { cout << "[incrementTask from " << context.str() << "]" << endl; for (int i = 0; i < 200 && !eventQueue.isTerminated(); i++) { context.parameters ().counter++; cout ().counter << endl; boost::this_thread::sleep(boost::posix_time::milliseconds(100)); } return TaskResult::SUCCESS(); }

57. (C)2014 Roi Yehoshua The counting stops when reaching 100:

58. (C)2014 Roi Yehoshua When two or more tasks are running concurrently, you need to take care of synchronizing their access to variables in the world model In the following example we have two tasks – incrementTask and decrementTask that are running in parallel and are both accessing the same counter variable in the world model

59. (C)2014 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 ()

60. (C)2014 Roi Yehoshua TAO(Incrementer) { TAO_PLANS { Increment } TAO_START_PLAN(Increment); TAO_BGN { TAO_PLAN(Increment) { TAO_START_CONDITION(true); TAO_CALL_TASK(incrementTask); TAO_CALL_TASK(decrementTask); TAO_ALLOCATE_EMPTY TAO_STOP_CONDITION(true); TAO_NEXT_EMPTY } TAO_END } TAO(Incrementer) { TAO_PLANS { Increment } TAO_START_PLAN(Increment); TAO_BGN { TAO_PLAN(Increment) { TAO_START_CONDITION(true); TAO_CALL_TASK(incrementTask); TAO_CALL_TASK(decrementTask); TAO_ALLOCATE_EMPTY TAO_STOP_CONDITION(true); TAO_NEXT_EMPTY } TAO_END }

61. (C)2014 Roi Yehoshua TaskResult incrementTask(string name, const CallContext& context, EventQueue& eventQueue) { cout << "[incrementTask from " << context.str() << "]" << endl; for (int i = 0; i < 1000000; i++) { context.parameters ().counter++; } ROS_INFO("Increment task finished"); return TaskResult::SUCCESS(); } TaskResult decrementTask(string name, const CallContext& context, EventQueue& eventQueue) { cout << "[decrementTask from " << context.str() << "]" << endl; for (int i = 0; i < 1000000; i++) { context.parameters ().counter--; } ROS_INFO("Decrement task finished"); return TaskResult::SUCCESS(); } TaskResult incrementTask(string name, const CallContext& context, EventQueue& eventQueue) { cout << "[incrementTask from " << context.str() << "]" << endl; for (int i = 0; i < 1000000; i++) { context.parameters ().counter++; } ROS_INFO("Increment task finished"); return TaskResult::SUCCESS(); } TaskResult decrementTask(string name, const CallContext& context, EventQueue& eventQueue) { cout << "[decrementTask from " << context.str() << "]" << endl; for (int i = 0; i < 1000000; i++) { context.parameters ().counter--; } ROS_INFO("Decrement task finished"); return TaskResult::SUCCESS(); }

62. (C)2014 Roi Yehoshua int main(int argc, char **argv) { ros::init(argc, argv, "tasks_sync"); ros::NodeHandle nh; ros_decision_making_init(argc, argv); ros::AsyncSpinner spinner(1); spinner.start(); LocalTasks::registrate("incrementTask", incrementTask); LocalTasks::registrate("decrementTask", decrementTask); … eventQueue.async_spin(); ROS_INFO("Starting incrementer plan..."); TaoIncrementer(&context, &eventQueue); eventQueue.close(); ROS_INFO("counter: %d", context.parameters ().counter); ROS_INFO("TAO finished."); return 0; } int main(int argc, char **argv) { ros::init(argc, argv, "tasks_sync"); ros::NodeHandle nh; ros_decision_making_init(argc, argv); ros::AsyncSpinner spinner(1); spinner.start(); LocalTasks::registrate("incrementTask", incrementTask); LocalTasks::registrate("decrementTask", decrementTask); … eventQueue.async_spin(); ROS_INFO("Starting incrementer plan..."); TaoIncrementer(&context, &eventQueue); eventQueue.close(); ROS_INFO("counter: %d", context.parameters ().counter); ROS_INFO("TAO finished."); return 0; }

63. (C)2014 Roi Yehoshua Each time the program is run, we get different results

64. To make the tasks synchronized we will define a mutex variable in the world model and use it to lock the access to the shared variable Add the following variable to the world model: (C)2014 Roi Yehoshua struct WorldModel : public CallContextParameters { boost::mutex mtx; // for synchronized update int counter; string str() const { stringstream s; s << "counter=" << counter; return s.str(); } }; struct WorldModel : public CallContextParameters { boost::mutex mtx; // for synchronized update int counter; string str() const { stringstream s; s << "counter=" << counter; return s.str(); } };

65. In the tasks functions declare a scoped_lock before accessing the shared variable (C)2014 Roi Yehoshua TaskResult incrementTask(string name, const CallContext& context, EventQueue& eventQueue) { cout << "[incrementTask from " << context.str() << "]" << endl; for (int i = 0; i < 1000000; i++) { boost::mutex::scoped_lock locker (context.parameters ().mtx); context.parameters ().counter++; } context.parameters ().incrementTaskFinished = true; return TaskResult::SUCCESS(); } TaskResult decrementTask(string name, const CallContext& context, EventQueue& eventQueue) { cout << "[decrementTask from " << context.str() << "]" << endl; for (int i = 0; i < 1000000; i++) { boost::mutex::scoped_lock locker (context.parameters ().mtx); context.parameters ().counter--; } context.parameters ().decrementTaskFinished = true; return TaskResult::SUCCESS(); } TaskResult incrementTask(string name, const CallContext& context, EventQueue& eventQueue) { cout << "[incrementTask from " << context.str() << "]" << endl; for (int i = 0; i < 1000000; i++) { boost::mutex::scoped_lock locker (context.parameters ().mtx); context.parameters ().counter++; } context.parameters ().incrementTaskFinished = true; return TaskResult::SUCCESS(); } TaskResult decrementTask(string name, const CallContext& context, EventQueue& eventQueue) { cout << "[decrementTask from " << context.str() << "]" << endl; for (int i = 0; i < 1000000; i++) { boost::mutex::scoped_lock locker (context.parameters ().mtx); context.parameters ().counter--; } context.parameters ().decrementTaskFinished = true; return TaskResult::SUCCESS(); }

66. (C)2014 Roi Yehoshua Now the counter is always 0 at the end:

67. Create a TAO plan for a single robot which needs to wander in the environment until it detects an obstacle in front of it (in a distance of 0.5m) and then it stops Run the TAO plan on a Lizi robot in Gazebo (C)2014 Roi Yehoshua