1 / 67

Multi-Robot Systems with ROS Lesson 9

Multi-Robot Systems with ROS Lesson 9. Teaching Assistant: Roi Yehoshua roiyeho@gmail.com. Agenda. Decision making in ROS TAO plans World model Tasks Runtime inspection of DM models. Decision Making. http://wiki.ros.org/decision_making

honora
Download Presentation

Multi-Robot Systems with ROS Lesson 9

An Image/Link below is provided (as is) to download presentation Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author. Content is provided to you AS IS for your information and personal use only. Download presentation by click this link. While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server. During download, if you can't get a presentation, the file might be deleted by the publisher.

E N D

Presentation Transcript

  1. Multi-Robot Systems with ROS Lesson 9 Teaching Assistant: RoiYehoshua roiyeho@gmail.com

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

  3. Decision Making • 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 (C)2014 Roi Yehoshua

  4. Installation • 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. Installation • 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 • $ 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 (C)2014 Roi Yehoshua

  6. Installation • If you have changed the ROS_PACKAGE_PATH variable in .bashrc, make sure its definition is at the end of the file: • # 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' (C)2014 Roi Yehoshua

  7. DM Examples • 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: • $ roslaunchdecision_making_examplesfsm_wandering.launch (C)2014 Roi Yehoshua

  8. FSM Wandering Example (C)2014 Roi Yehoshua

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

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

  11. Formation Example • For example, to launch the formation example type: • $ roslaunchdm_teamwork_examplesformation.launch (C)2014 Roi Yehoshua

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

  13. CogniTAO • CogniTAO (Think As One) is an implementation of the BDI architecture for both single robot missions and for multiple robots working in teams • 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. Plan • Defines the current Task to be performed, contains Start and Stop conditions, and is coupled with corresponding Plans through Allocation and Next protocols (C)2014 Roi Yehoshua

  15. TAO • 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 (C)2014 Roi Yehoshua

  16. Start and Stop Conditions • 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) (C)2014 Roi Yehoshua

  17. Next Protocol • 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 (C)2014 Roi Yehoshua

  18. Allocation Protocol • 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 (C)2014 Roi Yehoshua

  19. Next and Allocation Protocols in Teams • 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 (C)2014 Roi Yehoshua

  20. TAO Machine Definition • 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 • } (C)2014 Roi Yehoshua

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

  22. TAO Plan Definition • Must be located inside a TAO_BGN-TAO_END block • All task calls after TAO_ALLOCATION section run in parallel as the plan begins 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 } Validated before plan is selectedfor running • Validated all the time while the plan is running Tasks called upon plan execution Allocate sub-plans Optional exit actions • After STOP_CONDITION is satisfied, apply PROTOCOL to select next plan for execution (C)2014 Roi Yehoshua

  23. TAO Protocols • 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), • ...   • } (C)2014 Roi Yehoshua

  24. TAO Protocols • 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), • ...   • } (C)2014 Roi Yehoshua

  25. Basic Plan Example • 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: • $ cddmw/src • $ catkin_create_pkgtao_plansroscppdecision_makingdecision_making_parser (C)2014 Roi Yehoshua

  26. Basic Plan Example • 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" (C)2014 Roi Yehoshua

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

  28. BasicPlan.cpp (1) #include <iostream> #include <ros/ros.h> #include <decision_making/TAO.h> #include <decision_making/TAOStdProtocols.h> #include <decision_making/ROSTask.h> #include <decision_making/DecisionMaking.h> using namespace std; using namespace decision_making; int counter = 0; (C)2014 Roi Yehoshua

  29. BasicPlan.cpp (2) 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)2014 Roi Yehoshua

  30. BasicPlan.cpp (3) int main(intargc, char **argv) { ros::init(argc, argv, "basic_plan"); ros::NodeHandlenh; ros_decision_making_init(argc, argv); ros::AsyncSpinner spinner(1); spinner.start(); RosEventQueueeventQueue; 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; } (C)2014 Roi Yehoshua

  31. Executing TAO • 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) • TaskResultTaoNAME(const CallContext* ctx, EventQueue* eventQueue) (C)2014 Roi Yehoshua

  32. CallContext • 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 • boolisParametersDefined() const - check if parameters have been created before • Context is used for teamwork as well (more on this later) (C)2014 Roi Yehoshua

  33. Running Incrementer Plan • Add the following lines to CMakeLists.txt • Build the package by calling catkin_make • add_executable(basic_plansrc/BasicPlan.cpp) • target_link_libraries(basic_plan ${catkin_LIBRARIES}) (C)2013 Roi Yehoshua

  34. Running Incrementer Plan • Use rosrun to run the node: (C)2013 Roi Yehoshua

  35. Runtime Inspection of DM Models • 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)2014 Roi Yehoshua

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

  37. Runtime Inspection of DM Models • Incrementer plan’s graph (incrementer.dot.gif): (C)2014 Roi Yehoshua

  38. Runtime Inspection of DM Models • Plan converted to XML: • <tao id="/Incrementer" name="Incrementer" start="/Incrementer/Increment"> • <tao_plans id="/Incrementer/plans"> • <tao_plan id="/Incrementer/Increment" name="Increment"> • <tao_start_condition id="/Incrementer/Increment/start"> • <![CDATA[true]]> • </tao_start_condition> • <tao_stop_condition id="/Incrementer/Increment/stop"> • <![CDATA[counter < 100]]> • </tao_stop_condition> • <tao_next id="/Incrementer/Increment" protocol="NextFirstReady"> • <tao_next_op id="/Incrementer/Increment/Increment" name="/Incrementer/Increment" /> • </tao_next> • </tao_plan> • </tao_plans> • </tao> (C)2014 Roi Yehoshua

  39. Runtime Inspection of DM Models • In order to see visually the model in runtime, use the Decision Making rqtplugin • First run rqt: • Choose the Decision Making graph plugin: • $ rqt (C)2014 Roi Yehoshua

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

  41. World Model • 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)2014 Roi Yehoshua

  42. BasicPlanWithWM.cpp (1) structWorldModel : publicCallContextParameters { int counter; string str() const { stringstream s; s << "counter=" << counter; return s.str(); } }; #define WM TAO_CONTEXT.parameters<WorldModel>() (C)2014 Roi Yehoshua

  43. BasicPlanWithWM.cpp (2) 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)2014 Roi Yehoshua

  44. BasicPlanWithWM.cpp (3) int main(intargc, char **argv) { ros::init(argc, argv, "basic_plan_wm"); ros::NodeHandlenh; ros_decision_making_init(argc, argv); ros::AsyncSpinner spinner(1); spinner.start(); RosEventQueueeventQueue; CallContext context; context.createParameters(newWorldModel());  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; } (C)2014 Roi Yehoshua

  45. Running Basic Plan with World Model • Use rosrun to run the node: (C)2013 Roi Yehoshua

  46. Tasks • 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 (C)2014 Roi Yehoshua

  47. Task Registration and Invocation • 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); (C)2014 Roi Yehoshua

  48. Long Running Tasks • 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(); • } (C)2014 Roi Yehoshua

  49. Task Example • 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 (C)2014 Roi Yehoshua

  50. BasicPlanWithTasks.cpp (1) 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 } (C)2014 Roi Yehoshua

More Related