multi-robot systems with ros lesson 7

Multi-Robot Systems with ROS Lesson 7

Teaching Assistant: Roi Yehoshuaroiyeho@gmail.com

Summer 2015

(C)2015 Roi Yehoshua

Agenda• Basic TAO plan example• Runtime inspection of DM models• World model• Tasks

(C)2015 Roi Yehoshua

TAO Plan – Reminder

(C)2015 Roi Yehoshua

TAO Plan Definition

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 selectedfor running

Validated all the time while the plan is running

(C)2015 Roi Yehoshua

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:$ cd dmw/src$ catkin_create_pkg tao_plans roscpp decision_making decision_making_parser

(C)2015 Roi Yehoshua

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)2015 Roi Yehoshua

Basic Plan Example• Add a C++ source file to the project called

BasicPlan.cpp

(C)2015 Roi Yehoshua

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)2015 Roi Yehoshua

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)2015 Roi Yehoshua

BasicPlan.cpp (3)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

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)

TaskResult TaoNAME(const CallContext* ctx, EventQueue* eventQueue)

(C)2015 Roi Yehoshua

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– bool isParametersDefined() const - check if parameters have

been created before• Context is used for teamwork as well (more on this later)

(C)2015 Roi Yehoshua

Running Incrementer Plan• Add the following lines to CMakeLists.txt

• Build the package by calling catkin_make

add_executable(basic_plan src/BasicPlan.cpp)target_link_libraries(basic_plan ${catkin_LIBRARIES})

(C)2015 Roi Yehoshua

Running Incrementer Plan• Use rosrun to run the node:

(C)2015 Roi Yehoshua

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)2015 Roi Yehoshua

Runtime Inspection of DM Models• The xml and dot files will be created in the folder

~/dmw/devel/share/tao_plans/graphs

(C)2015 Roi Yehoshua

Runtime Inspection of DM Models• Incrementer plan’s graph (incrementer.dot.gif):

(C)2015 Roi Yehoshua

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)2015 Roi Yehoshua

Runtime Inspection of DM Models• 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

(C)2015 Roi Yehoshua

• Once the model is running, the visualization of your model should be loaded automatically

Runtime Inspection of DM Models

(C)2015 Roi Yehoshua

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)2015 Roi Yehoshua

BasicPlanWithWM.cpp (1)struct WorldModel : public CallContextParameters { int counter;  string str() const { stringstream s; s << "counter=" << counter; return s.str(); }};#define WM TAO_CONTEXT.parameters<WorldModel>()

(C)2015 Roi Yehoshua

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)2015 Roi Yehoshua

BasicPlanWithWM.cpp (3)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;}

(C)2015 Roi Yehoshua

Running Basic Plan with World Model• Use rosrun to run the node:

(C)2014 Roi Yehoshua

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

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

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

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

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

• Task callback function:

BasicPlanWithTasks.cpp (2)

TaskResult incrementTask(string name, const CallContext& context, EventQueue& eventQueue) { cout << "[incrementTask from " << context.str() << "]" << endl;  context.parameters<WorldModel>().counter++; cout << "counter: " << context.parameters<WorldModel>().counter << endl; boost::this_thread::sleep(boost::posix_time::milliseconds(100));  return TaskResult::SUCCESS();}

(C)2014 Roi Yehoshua

• Task registration:

BasicPlanWithTasks.cpp (3)

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;}

(C)2013 Roi Yehoshua

Running Basic Plan with Tasks

(C)2014 Roi Yehoshua

Tasks And Stop Conditions• 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

(C)2014 Roi Yehoshua

TaskWithStopCondition.cpp (1)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}

(C)2014 Roi Yehoshua

TaskWithStopCondition.cpp (2)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<WorldModel>().counter++; cout << "counter: " << context.parameters<WorldModel>().counter << endl; boost::this_thread::sleep(boost::posix_time::milliseconds(100)); }  return TaskResult::SUCCESS();}

(C)2014 Roi Yehoshua

Task With Stop Condition• The counting stops when reaching 100:

(C)2014 Roi Yehoshua

Tasks Synchronization• 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

(C)2014 Roi Yehoshua

TasksSync.cpp (1)struct WorldModel : public CallContextParameters { int counter;  string str() const { stringstream s; s << "counter=" << counter; return s.str(); }};#define WM TAO_CONTEXT.parameters<WorldModel>()

(C)2014 Roi Yehoshua

TasksSync.cpp (2)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}

(C)2014 Roi Yehoshua

TasksSync.cpp (3)TaskResult incrementTask(string name, const CallContext& context, EventQueue& eventQueue) { cout << "[incrementTask from " << context.str() << "]" << endl;  for (int i = 0; i < 1000000; i++) { context.parameters<WorldModel>().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<WorldModel>().counter--; }  ROS_INFO("Decrement task finished"); return TaskResult::SUCCESS();}

(C)2014 Roi Yehoshua

TasksSync.cpp (4)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<WorldModel>().counter); ROS_INFO("TAO finished.");  return 0;}

(C)2014 Roi Yehoshua

Tasks Synchronization• Each time the program is run, we get different

results

(C)2014 Roi Yehoshua

• 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:

Tasks Synchronization

struct WorldModel : public CallContextParameters { boost::mutex mtx; // for synchronized update int counter;  string str() const { stringstream s; s << "counter=" << counter; return s.str(); }};

(C)2014 Roi Yehoshua

• In the tasks functions declare a scoped_lock before accessing the shared variable

Tasks Synchronization

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<WorldModel>().mtx); context.parameters<WorldModel>().counter++; }  context.parameters<WorldModel>().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<WorldModel>().mtx); context.parameters<WorldModel>().counter--; }  context.parameters<WorldModel>().decrementTaskFinished = true; return TaskResult::SUCCESS();}

(C)2014 Roi Yehoshua

Tasks Synchronization• Now the counter is always 0 at the end:

(C)2014 Roi Yehoshua

Homework (not for submission)• 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