Note: This tutorial assumes that you have completed the previous tutorials: decision_making/Tutorials/CogniTAO(C++).
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Getting Started with TAO in C++

Description: TAO, Think As One

Keywords: decision_making,TAO

Tutorial Level: BEGINNER

Introduction

As described in CogniTAO BDI, a number of components (Plans, Tasks, Allocations) are written and designed by the programmer, while complying with the TAO Plans and hierarchy model. Here we will demonstrate how to put the pieces together, and create your first TAO program using those components, in an easy-to-catch tutorial that will cover most of what you need to understand, in order to comprehend the easy usage of TAO.

Running the example

roslaunch decision_making_examples tao_increment.launch

Components, Conditions And Code

This is taoIncrement.cpp - In the following code we demonstrate the TAO Incremental example (hold on - it looks like much - so we'll break it down to pieces in a moment...).

   1 #include <iostream>
   2 #include <boost/bind.hpp>
   3 #include <boost/thread.hpp>
   4 #include <boost/foreach.hpp>
   5 #include <ros/ros.h>
   6 
   7 #include <decision_making/TAO.h>
   8 #include <decision_making/TAOStdProtocols.h>
   9 #include <decision_making/ROSTask.h>
  10 #include <decision_making/DecisionMaking.h>
  11 
  12 using namespace std;
  13 using namespace decision_making;
  14 
  15 #define foreach BOOST_FOREACH
  16 
  17 TAO_HEADER(Even)
  18 TAO_HEADER(Odd)
  19 
  20 //#define TAO_STOP_CONDITION(X) TAO_STOP_CONDITION_AND_PRINT_EVENTS(X)
  21 
  22 struct WorldModel: public CallContextParameters{
  23         int i;
  24         string str()const{ stringstream s; s<<"i="<<i; return s.str(); }
  25 };
  26 #define WM call_ctx.parameters<WorldModel>()
  27 
  28 TAO(Incrementer)
  29 {
  30     TAO_PLANS{
  31         Increment,
  32     }
  33     TAO_START_PLAN(Increment);
  34     TAO_BGN{
  35         TAO_PLAN( Increment ){
  36             TAO_START_CONDITION( WM.i < 100 );
  37             TAO_ALLOCATE(AllocFirstReady){
  38                 TAO_SUBPLAN(Even);
  39                 TAO_SUBPLAN(Odd);
  40             }
  41             TAO_STOP_CONDITION( false );
  42             TAO_NEXT(NextFirstReady){
  43                 TAO_NEXT_PLAN(Increment);
  44             }
  45         }
  46     }
  47     TAO_END
  48 }
  49 
  50 TAO(Even)
  51 {
  52     TAO_PLANS{
  53         Even,
  54     }
  55     TAO_START_PLAN(Even);
  56     TAO_BGN
  57     {
  58         TAO_PLAN( Even ){
  59             TAO_START_CONDITION( WM.i%2 == 0 );
  60             WM.i++;
  61             cout<<"Even : "<<WM.str()<<endl;
  62             sleep(1);
  63             TAO_ALLOCATE_EMPTY
  64             TAO_STOP_CONDITION(true);
  65             TAO_NEXT_EMPTY
  66         }
  67     }
  68     TAO_END
  69 }
  70 
  71 TAO(Odd)
  72 {
  73     TAO_PLANS{
  74         Odd,
  75     }
  76     TAO_START_PLAN(Odd);
  77     TAO_BGN
  78     {
  79         TAO_PLAN( Odd ){
  80             TAO_START_CONDITION( WM.i%2 != 0 );
  81             WM.i++;
  82             cout<<"Odd : "<<WM.str()<<endl;
  83             sleep(1);
  84             TAO_ALLOCATE_EMPTY
  85             TAO_STOP_CONDITION(true);
  86             TAO_NEXT_EMPTY
  87         }
  88     }
  89     TAO_END
  90 }
  91 
  92 decision_making::TaskResult testTask(string name, const FSMCallContext& context, EventQueue& eventQueue) {
  93         cout<<"[testTask from "<<context.str()<<"]"<<endl;
  94     sleep(1);
  95     return TaskResult::SUCCESS();
  96 }
  97 
  98 int main(int argc, char **argv) {
  99 
 100     ros::init(argc, argv, "tao_example_incrementer");
 101 
 102     ROS_INFO("Starting...");
 103 
 104     ros_decision_making_init(argc, argv);
 105     ros::NodeHandle nodeHandle;
 106     RosEventQueue eventQueue;
 107 
 108     ros::AsyncSpinner spinner(1);
 109     spinner.start();
 110 
 111     LocalTasks::registrate("testTask", testTask);
 112 
 113     eventQueue.async_spin();
 114     CallContext call_ctx;
 115     call_ctx.createParameters(new WorldModel());
 116     TaoIncrementer(&call_ctx, &eventQueue);
 117     eventQueue.close();
 118     ROS_INFO("Finished.");
 119         return 0;
 120 }

Now let's break it down.

Our Plans are 'Incremental', 'Even', and 'Odd', Start and Stop conditions are Boolean as mentioned in the previous tutorial, and we have only one Allocation, for the Incremental plan.

These are the obvious inclusions, the TAO_HEADER for Odd and Even are mentioned here for C++ logic, so they will be known beforehand (just so everything will be recognized, so don't forget any of them!):

here we go:

   1 #include <iostream>
   2 #include <boost/bind.hpp>
   3 #include <boost/thread.hpp>
   4 #include <boost/foreach.hpp>
   5 #include <ros/ros.h>
   6 
   7 #include <decision_making/TAO.h>
   8 #include <decision_making/TAOStdProtocols.h>
   9 #include <decision_making/ROSTask.h>
  10 #include <decision_making/DecisionMaking.h>
  11 
  12 using namespace std;
  13 using namespace decision_making;
  14 
  15 #define foreach BOOST_FOREACH
  16 
  17 TAO_HEADER(Even)
  18 TAO_HEADER(Odd)

A definition of the context parameters for our world model, which will be used in our Incremental START conditions:

   1 struct WorldModel: public CallContextParameters{
   2         int i;
   3         string str()const{ stringstream s; s<<"i="<<i; return s.str(); }
   4 };
   5 #define WM call_ctx.parameters<WorldModel>()
   6 

And now for the interesting part..

This is where TAO really comes in - We define the TAO plans for each component (according toTAO C++ reference as follows:

First for Incremental, our condition will be to run for 100 times (our context parameter will be incremented by our sub-plans).

notice the NEXT is the Incremental plan again, and our sub-plans are Odd and Even.

   1 TAO(Incrementer)
   2 {
   3     TAO_PLANS{
   4         Increment,
   5     }
   6     TAO_START_PLAN(Increment);
   7     TAO_BGN{
   8         TAO_PLAN( Increment ){
   9             TAO_START_CONDITION( WM.i < 100 );
  10             TAO_ALLOCATE(AllocFirstReady){
  11                 TAO_SUBPLAN(Even);
  12                 TAO_SUBPLAN(Odd);
  13             }
  14             TAO_STOP_CONDITION( false );
  15             TAO_NEXT(NextFirstReady){
  16                 TAO_NEXT_PLAN(Increment);
  17             }
  18         }
  19     }
  20     TAO_END
  21 }

And now for Even and Odd - after our Start condition we'll increment the context parameter, notice these are the last plans, so they have no TAO_ALLOCATE and no TAO_NEXT, we use the _EMPTY option instead.

Here we go:

   1 TAO(Even)
   2 {
   3     TAO_PLANS{
   4         Even,
   5     }
   6     TAO_START_PLAN(Even);
   7     TAO_BGN
   8     {
   9         TAO_PLAN( Even ){
  10             TAO_START_CONDITION( WM.i%2 == 0 );
  11             WM.i++;
  12             cout<<"Even : "<<WM.str()<<endl;
  13             sleep(1);
  14             TAO_ALLOCATE_EMPTY
  15             TAO_STOP_CONDITION(true);
  16             TAO_NEXT_EMPTY
  17         }
  18     }
  19     TAO_END
  20 }
  21 
  22 TAO(Odd)
  23 {
  24     TAO_PLANS{
  25         Odd,
  26     }
  27     TAO_START_PLAN(Odd);
  28     TAO_BGN
  29     {
  30         TAO_PLAN( Odd ){
  31             TAO_START_CONDITION( WM.i%2 != 0 );
  32             WM.i++;
  33             cout<<"Odd : "<<WM.str()<<endl;
  34             sleep(1);
  35             TAO_ALLOCATE_EMPTY
  36             TAO_STOP_CONDITION(true);
  37             TAO_NEXT_EMPTY
  38         }
  39     }
  40     TAO_END
  41 }

And for some basic running orders..

In the last part we'll define testTask for our result output, and run our main procedure: initialize ros and our program node, start the thread spinner, register the tasks, and call our TAO plan with the defined context parameters.

   1 decision_making::TaskResult testTask(string name, const FSMCallContext& context, EventQueue& eventQueue) {
   2         cout<<"[testTask from "<<context.str()<<"]"<<endl;
   3     sleep(1);
   4     return TaskResult::SUCCESS();
   5 }
   6 
   7 int main(int argc, char **argv) {
   8 
   9     ros::init(argc, argv, "tao_example_incrementer");
  10 
  11     ROS_INFO("Starting...");
  12 
  13     ros_decision_making_init(argc, argv);
  14     ros::NodeHandle nodeHandle;
  15     RosEventQueue eventQueue;
  16 
  17     ros::AsyncSpinner spinner(1);
  18     spinner.start();
  19 
  20     LocalTasks::registrate("testTask", testTask);
  21 
  22     eventQueue.async_spin();
  23     CallContext call_ctx;
  24     call_ctx.createParameters(new WorldModel());
  25     TaoIncrementer(&call_ctx, &eventQueue);
  26     eventQueue.close();
  27     ROS_INFO("Finished.");
  28         return 0;
  29 }

And that's it!

Using rqt_decision_graph

now let's run our rqt_decision_graph in parallel, to see the diagram our TAO program gives out:

TAO Incremental example

Wiki: decision_making/Tutorials/Increment TAO (last edited 2014-01-21 13:52:47 by TommySM)