multi-robot systems with ros lesson 7

Multi-Robot Systems with ROS Lesson 7

Teaching Assistant: Roi [email protected]

Summer 2015

mailto:[email protected]

(C)2015 Roi Yehoshua

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


TAO Plan – Reminder


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


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


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"


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

BasicPlan.cpp


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;


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


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


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)


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)


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


Running Incrementer Plan• Use rosrun to run the node:


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)


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

~/dmw/devel/share/tao_plans/graphs


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


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>


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


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

Runtime Inspection of DM Models


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


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>()


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}


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


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


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


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


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


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


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}


• 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();}


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


Running Basic Plan with Tasks


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


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}


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


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


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


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>()


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}


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


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


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

results


• 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(); }};


• 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();}


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


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

multi-robot systems with ros lesson 7

Documents

tasktaskname tao

planincrement tao

condition tao

pkg tao

allocation tao

tao machinesit

plan executionafterstop

project index