A task scheduler for ROS

Cédric Pradalier

To cite this version:

Cédric Pradalier. A task scheduler for ROS. [Research Report] UMI 2958 GeorgiaTech-CNRS. 2017.
�hal-01435823v4�

HAL Id: hal-01435823

https://hal.science/hal-01435823v4
Submitted on 29 Dec 2020

HAL is a multi-disciplinary open access L’archive ouverte pluridisciplinaire HAL, est

archive for the deposit and dissemination of sci- destinée au dépôt et à la diffusion de documents
entific research documents, whether they are pub- scientifiques de niveau recherche, publiés ou non,
lished or not. The documents may come from émanant des établissements d’enseignement et de
teaching and research institutions in France or recherche français ou étrangers, des laboratoires
abroad, or from public or private research centers. publics ou privés.

Distributed under a Creative Commons Attribution| 4.0 International License

 A task scheduler for ROS
Cédric Pradalier
UMI 2958 GT-CNRS - GeorgiaTech Lorraine
Metz, France
December 29, 2020

Contents
1 Introduction 2

2 Concepts 2
2.1 What is a task? . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2
2.2 A basic example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.3 Purpose of the task system . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
2.4 Task parameters and Dynamic Reconfigure . . . . . . . . . . . . . . . . . . . . . . 4
2.5 Task Environment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.6 Templated Parents . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
2.7 The Task Server . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

3 Creating a new task framework 6

3.1 Required files . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
3.2 The Environment . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
3.3 The task server . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
3.4 Task Idle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
3.5 An example: Task GoTo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
3.6 Implementation Constraints . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

4 Using the task framework 12

4.1 Compilation and linking . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
4.2 Launch file . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
4.3 Console . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
4.4 Simple missions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
4.5 Background tasks . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
4.6 Interruptions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

5 Integration with other frameworks 15

5.1 Integration with Smach . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
5.2 Integration with the ActionLib . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
5.3 Integration with MoveBase . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17
5.4 Tasks and multi-agent synchronization . . . . . . . . . . . . . . . . . . . . . . . . . 18

6 Acknowledgements 22

1
1 Introduction
Developing a complete robotic system often requires combining multiple behaviours into a complex
decision grid, with elements running in sequence or in parallel, eventually interrupting each others.
To solve this “age-old” problem, ROS provides two main tools:
Actionlib: a client-server architecture that provides a way to specify results to be achieved.
While the server works on these results, it should report progresses and ultimately report
when the task is completed.

Smach: a python API to define complex state machines. It can interact with ROS services and
actions to define a complex behaviour, including nesting, interruptions and concurrence.
Combining Smach and Actionlib, one could build arbitrarily complex systems. Hence, why would
another task management system be necessary?
The main argument in favour of our task scheduler is the simplicity of its use, particularly in
comparison with Actionlib. As usual, simplicity is a trade-off against expressiveness. Simplicity
can be sacrificed by linking our task scheduler with Actionlib and/or Smach to exploit the best of
both worlds.
This task scheduling framework is the culmination of about 10 years of experience developing
robotic applications in the academic context. Most applications we designed could be handled
using the task scheduling framework we will present in this document.
We provide the source code for this project at: https://github.com/cedricpradalier/ros_
task_manager.

2 Concepts
2.1 What is a task?
In our framework a task is seen as a behaviour that can be started, run for some amount of time,
possibly infinite, and then terminates either on completion or on interruption. A task requires
some context such as system variables, ROS topics, and services, but it can also store its own
internal variables and state.
Programmatically, a task is a C++ class that must implement an iterate function and may
implement an initialize and terminate function. We distinguish two concepts:

Task Definition: the main description of a task such as its name, help text and parameter
description, as well as some status information. It acts as a factory to instantiate the task
for specific parameters and is used to communicate with the scheduler clients. For a given
task name, a single task definition is possible.
Task Instance: a task instance is created from the task definition when the task needs to be run.
This is the class that needs to implement the initialize, iterate, and terminate function as
well as store any relevant variable. Multiple task instances with the same name and possibly
different parameters may be launched from a single task definition.
During the life of a task instance, the task will report different status, defined in the ROS
TaskStatus.msg file:

TASK NEWBORN: The task has been created from the task definition.
TASK CONFIGURED: The task has been configured by the task definition. This status is
inherited from previous version and not used anymore.
TASK INITIALISED: The task instance initialisation function has been called and returned
successfully.

2
 TASK RUNNING: The task instance iterate function returns this when the task needs to keep
running. This is only relevant for tasks whose loop is managed by the task scheduler.
TASK COMPLETED: The task instance iterate function returns this when the task objective
has been met. The iterate function will not be called anymore.
TASK TERMINATED: Returned by the task instance terminate function. In addition, this
status is added as a binary mask to any error status described below so it should be tested
as status & TASK TERMINATED in C.
Task instance will sometime fail or be interrupted before reporting completion. The status
code is then a combination of TASK TERMINATED and the following code:
TASK INTERRUPTED: The task was interrupted by the task scheduler.
TASK FAILED: The task reported failure in the iterate function.
TASK TIMEOUT: The task instance execution timeout was passed and the task was stopped by
the task scheduler.
TASK CONFIGURATION FAILED: The configuration stage failed (unused).
TASK INITIALISATION FAILED: Can be returned by the initialization function when it failed.
Should one of the task function return one of the status above, it will be terminated immediately.
In addition, to the task status, a task can use the setStatusString function to document the
cause of failure.

2.2 A basic example

For an initial example, we create a simple task for reaching a desired destination (let us ignore
the template parameters for now).
1 class Ta sk F ac to r yG oT o : public TaskDefinition < TaskGoToConfig , TurtleSimEnv , TaskGoTo >
2 {
3
4 public :
5 T as kF ac t or yG o To ( T a s k E n v i r o n m e n t P t r env ) :
6 Parent ( " GoTo " ," Reach a desired destination " , true , env ) {}
7 virtual ~ Ta s kF ac t or yG oT o () {};
8 };

Using this task definition, we next create a specific task instance:

1 class TaskGoTo : public TaskInstance < TaskGoToConfig , TurtleSimEnv >
2 {
3 public :
4 TaskGoTo ( T a s k D e f i n i t i o n P t r def , T a s k E n v i r o n m e n t P t r env ) : Parent ( def , env ) {}
5 virtual ~ TaskGoTo () {};
6
7 virtual TaskIndicator iterate () ;
8
9 virtual TaskIndicator terminate () ;
10 };

The implementation of this particular task just need to focus on the specifics of the behaviour,
taking advantage of some parameters available in the cfg variable.
1 TaskIndicator TaskGoTo :: iterate ()
2 {
3 const turtlesim :: Pose & tpose = env - > getPose () ;
4 // distance to target
5 double r = hypot ( cfg . goal_y - tpose .y , cfg . goal_x - tpose . x ) ;
6 // completion condition
7 if ( r < cfg . dist_ threshol d ) {
8 return TaskStatus :: TASK_COM PLETED ;
9 }
10 // angle to target
11 double alpha = remainder ( atan2 (( cfg . goal_y - tpose . y ) , cfg . goal_x - tpose . x ) - tpose . theta ,2*
M_PI ) ;

3
12 // Saturated proportional control law .
13 if ( fabs ( alpha ) > M_PI /6) {
14 double rot = (( alpha >0) ?+1: -1) * M_PI /6;
15 env - > p ub li sh V el oc it y (0 , rot ) ;
16 } else {
17 double vel = cfg . k_v * r ;
18 double rot = cfg . k_alpha * alpha ;
19 if ( vel > cfg . max_velocity ) vel = cfg . max_velocity ;
20 env - > p ub li sh V el oc it y ( vel , rot ) ;
21 }
22 return TaskStatus :: TASK_RUNNING ;
23 }

Finally, we implement the terminate function to ensure the last requested velocity is always zero.
1 TaskIndicator TaskGoTo :: terminate ()
2 {
3 env - > p ub li sh V el oc i ty (0 ,0) ;
4 return TaskStatus :: T AS K_ TE R MI NATE D ;
5 }

2.3 Purpose of the task system

The purpose of the task system is to facilitate the development and testing of complex applications.
In such an application, the system (i.e. the task server) will have a set of pre-implemented
behaviours that can be instantiated by a client application. In the simplest case, the combination
of behaviours is done in sequence: go to point A, take picture, go to point B, deliver coffee and
go back to charging station. We define such a sequence as a mission. The simplest missions are
statically defined but branching on some conditions could also be required: deliver coffee, if water
is empty, go refill, go back to charging station. As more and more conditions are required, a proper
state machine management could be implemented using Smach. In an academic setting though,
most demonstrations can be implemented with simple, human-readable, quasi-linear missions.
Our missions are typically implemented in python and look like the following script:
1 # !/ usr / bin / python
2 import roslib ; roslib . load_manifest ( ’ t a s k _ m a n a g e r _ t u r t l e s i m ’)
3 import rospy
4 from t a s k _ m a n a g e r _ l i b . TaskClient import *
5
6 rospy . init_node ( ’ task_client ’)
7 tc = TaskClient ( " / task_server " ,0.2)
8
9 while True :
10 tc . Wait ( duration =1.)
11 tc . GoTo ( goal_x =1.0 , goal_y =1.0)
12 tc . Wait ( duration =2.)
13 tc . GoTo ( goal_x =5.0 , goal_y =5.0)

In this listing, it is important to notice that the tasks are called as member functions of the
TaskClient, using the name defined in the TaskDefinition class. These functions are dynamically
generated from the information received from the task server. Because the server also provides the
parameters that the function can accept, parameters are order-independent and explicitly named
in the mission. This also allows checking and enforcing type compatibility before trying to execute
a task.

2.4 Task parameters and Dynamic Reconfigure

To allow introspection, the task parameters are defined using the dynamic reconfigure1 framework.
This allows specifying each parameter with a name, a help string, a type, a default value and a
range where appropriate. The dynamic reconfigure framework defines parameters in a config file
similar to the following:
1 # ! / usr / bin / env python
2 PACKAGE = ’ t a s k _ m a n a g e r _ t u r t l e s i m ’
3 import roslib ; roslib . load_manifest ( PACKAGE )

1 http://wiki.ros.org/dynamic_reconfigure/Tutorials

4
 4
5 from d y n a m i c _ r e c o n f i g u r e . p a r a m e t e r _ g e n e r a t o r import *
6 from t a s k _ m a n a g e r _ l i b . p a r a m e t e r _ g e n e r a t o r import *
7
8 gen = T a s k P a r a m e t e r G e n e r a t o r ()
9 # Name Type Description Default Min
Max
10 gen . add ( " goal_x " , double_t , 0, " X coordinate of destination " , 0.0)
11 gen . add ( " goal_y " , double_t , 0, " Y coordinate of destination " , 0.0)
12 gen . add ( " k_v " , double_t , 0, " Gain for velocity control " , 1.0)
13 gen . add ( " k_alpha " , double_t , 0, " Gain for angular control " , 1.0)
14 gen . add ( " max_velocity " , double_t , 0, " Max allowed velocity " , 1.0)
15 gen . add ( " d ist_thre shold " , double_t , 0, " Distance at which a the target is considered
reached " , 0.1)
16
17 exit ( gen . generate ( PACKAGE , PACKAGE , " TaskGoTo " ) )

Note that the ParameterGenerator class has been overloaded with the TaskParameterGenerator
to make sure default parameters common to all tasks are always present in the list.
A secondary benefit of using the dynamic reconfigure framework is that it is possible to use
the reconfigure gui to check the values of the parameters while a task is running and eventually
change them. This is particularly useful to tune control parameters at run-time.
In practice dynamic reconfigure generates a Python and a C++ class containing the parameter
data as class members. This class is one of the template parameters of the TaskDefinition and
TaskInstance classes. It is available in a variable named cfg in every instance, as can be observed
in the iterate function above.

2.5 Task Environment

In most applications, a common set of functions, variables and topics of interest will be needed by
most if not all classes. These could be robot dimensions, velocity commands, sensor measurements,
etc... As mentioned above, it is possible for each task to subscribe to its own topics when started
and unsubscribe when terminating. To simplify the development of functions shared between tasks
the task server shares a common variable called the task environment with every task. There are
no constraints regarding what can be included in the environment because the parent class only
provides a common mutex:
1 /* *
2 * Empty class , to be inherited for a specific application .
3 * */
4 class Ta sk E nv ir o nm en t {
5 public :
6 // This mutex will be locked in all the task instance function
7 // ( initialize , iterate , terminate ) for periodic tasks . However ,
8 // for non - periodic tasks , it is not possible to lock the
9 // environment forever , so the lock is taken only for initialize and
10 // terminate , but the user needs to organise his / her own locking as
11 // appropriate , e . g for a reader or a writer
12 // boost :: shared_lock < boost :: shared_mutex > guard ( env_gen - > e n v i r o n m e n t _ m u t e x ) ;
13 // boost :: unique_lock < boost :: shared_mutex > guard ( env_gen - > e n v i r o n m e n t _ m u t e x ) ;
14 boost :: shared_mutex e n v i r o n m e n t _ m u t e x ;
15 public :
16 T as kE nv i ro nm e nt () {}
17 virtual ~ Ta s kE nv ir o nm en t () {}
18 };

2.6 Templated Parents

To further simplify the implementation of new classes, common functions are written into a tem-
plated class, which is later inherited for specific applications.
TaskDefinition<Config,Environment,Instance> defines a new task factory, specialized on a
specific task parameter of type Config, a specific environment, and specific instances.
TaskInstance<Config,Environment> specializes a task instance for an application.

5
The Environment class is used to create a member variable env pointing to the shared environment.
The Config class is used to create a member variable cfg, which is filled with the task parameters
when the task is instantiated and updated as appropriate by the dynamic reconfigure framework.

2.7 The Task Server

The task server has multiple responsibilities in this framework. First, it loads the description of all
tasks (TaskDefinition objects). It then provides a service to start or stop a given task, and keeps
track of the status of all tasks currently running or recently terminated. It is also responsible for
instantiating the task scheduler that manages the threads in which tasks actually run. Because
none of this is application-specific, most of the task server can be made generic. As a result, the
main function for a specific task server will often be as simple as the following listing:
1 # include " t a s k _ m a n a g e r _ l i b / T a s k S e r v e r D e f a u l t . h "
2 # include " t a s k _ m a n a g e r _ t u r t l e s i m / TurtleSimEnv . h "
3
4 using namespace t a s k _m a n a g e r _ l i b ;
5
6 class TaskServer : public TaskSer verBase {
7 public :
8 TaskServer ( T a s k E n v i r o n m e n t P t r _env ) : TaskSe rverBas e ( _env , true ) {
9 start () ;
10 }
11 };
12
13 int main ( int argc , char * argv [])
14 {
15 ros :: init ( argc , argv , " tu rt le s im_t a sk s " ) ;
16 ros :: NodeHandle nh ( " ~ " ) ;
17 T a s k E n v i r o n m e n t P t r env ( new TurtleSimEnv ( nh ,1) ) ;
18 TaskServer ts ( env ) ;
19 ros :: spin () ;
20 return 0;
21 }

3 Creating a new task framework

To instantiate our framework through an example, we will start with the turtlesim simulator from
the ROS tutorial. We will then create a minimal set of tasks to implement part of the LOGO
functionalities2 .
The complete implementation is available in the example package task manager turtlesim.

3.1 Required files

Although the implementation-specific details will be given in the next sections, it is already possible
to list what is required to create a new task framework:
• Define a specialized task environment subscribing to commonly required topics and providing
common tools for all classes.
• Define a new task server main by mostly copy-pasting the above example.
• For each task, create a .cfg file to declare its parameters and define the task description
(inheriting from TaskDefinition) and the task instance (inheriting from TaskInstance).

Once a proper CMakeLists.txt has been created, all the tasks will be compiled as a shared library
and the task server will be able to dynamically load as many of them as it may find.
2 LOGO is an old programming language used to programmatically draw curves on screen in the early days of

computers: http://en.wikipedia.org/wiki/Logo_(programming_language)

6
3.2 The Environment
We set the environment up by adding function relevant to the application. For instance, it is
a fairly safe guess that any task working with the turtlesim environment will need to read the
turtle pose (turtlesim/Pose) and potentially publish velocity commands (turtlesim/Velocity).
These functions must then be part of the environment. We also add a helper function that uses
the turtlesim service to set the pen colour. This could arguably be subscribed to only in the task
that needs it. The TurtleSimEnv class is declared in the listing (header file) below:
1 // File : t a s k _ m a n a g e r _ t u r t l e s i m / TurtleSimEnv . h
2 # ifndef T U R T L E _ S I M _ E N V _ H
3 # define T U R T L E _ S I M _ E N V _ H
4
5 # include " t a s k _ m a n a g e r _ l i b / Ta skDefin ition . h "
6 # include " turtlesim / SetPen . h "
7 # include " turtlesim / Velocity . h "
8 # include " turtlesim / Pose . h "
9
10 namespace t a s k _ m a n a g e r _ t u r t l e s i m {
11 class TurtleSimEnv : public t a sk _ m a n a g e r _ l i b :: Ta s kE nv ir o nm en t
12 {
13 protected :
14 ros :: Subscriber poseSub ;
15 ros :: Publisher velPub ;
16 ros :: ServiceClient setPenClt ;
17
18 void poseCallback ( const turtlesim :: Pose :: ConstPtr & msg ) {
19 tpose = * msg ;
20 }
21
22 turtlesim :: Pose tpose ;
23
24 public :
25 TurtleSimEnv ( ros :: NodeHandle & nh ) ;
26 ~ TurtleSimEnv () {};
27
28 const turtlesim :: Pose & getPose () const ;
29
30 void p ub li sh V el oc i ty ( double linear , double angular ) ;
31
32 void setPen ( bool on , unsigned int r =0 xFF , unsigned int g =0 xFF ,
33 unsigned int b =0 xFF , unsigned int width =1) ;
34 };
35 };
36
37 # endif // T U R T L E _ SI M _ E N V _ H

Additionally, the required functions are defined in the following implementation file:
1 // File : TurtleSimEnv . cpp
2 # include " t a s k _ m a n a g e r _ t u r t l e s i m / TurtleSimEnv . h "
3
4 using namespace t a s k _ m a n a g e r _ t u r t l e s i m ;
5
6 TurtleSimEnv :: TurtleSimEnv ( ros :: NodeHandle & n ) : t a s k _ m a n a g e r _ l i b :: Ta sk E nv ir on m en t ( n )
7 {
8 setPenClt = nh . serviceClient < turtlesim :: SetPen >( " / turtle0 / set_pen " ) ;
9 poseSub = nh . subscribe ( " / turtle0 / pose " ,1 ,& TurtleSimEnv :: poseCallback , this ) ;
10 velPub = nh . advertise < turtlesim :: Velocity >( " / turtle0 / c o m m a n d _ v e l o c i t y " ,1) ;
11 }
12
13 const turtlesim :: Pose & TurtleSimEnv :: getPose () const
14 {
15 return tpose ;
16 }
17
18 void TurtleSim :: p u bl is hV e lo ci ty ( double linear , double angular ) {
19 turtlesim :: Velocity cmd ;
20 cmd . linear = linear ;
21 cmd . angular = angular ;
22 velPub . publish ( cmd ) ;
23 }
24
25 void TurtleSimEnv :: setPen ( bool on , unsigned int r , unsigned int g , unsigned int b , unsigned
int width )
26 {
27 turtlesim :: SetPen setpen ;
28 setpen . request . r = r ;

7
29 setpen . request . g = g ;
30 setpen . request . b = b ;
31 setpen . request . off = ! on ;
32 setpen . request . width = width ;
33 if (! setPenClt . call ( setpen ) ) {
34 ROS_ERROR ( " Failed to call service set_pen " ) ;
35 }
36 }

3.3 The task server

As mentioned earlier, the task server is mostly a copy-paste from the example task server:
1 // File : task_server . cpp
2 # include " t a s k _ m a n a g e r _ l i b / T a s k S e r v e r D e f a u l t . h "
3 # include " t a s k _ m a n a g e r _ t u r t l e s i m / TurtleSimEnv . h "
4
5 using namespace t a s k _m a n a g e r _ l i b ;
6
7 class TaskServer : public TaskSer verBase {
8 public :
9 TaskServer ( T a s k E n v i r o n m e n t P t r _env ) : TaskSe rverBas e ( _env , true ) {
10 start () ;
11 }
12 };
13
14 int main ( int argc , char * argv [])
15 {
16 ros :: init ( argc , argv , " tu rt le s im_t a sk s " ) ;
17 ros :: NodeHandle nh ( " ~ " ) ;
18 T a s k E n v i r o n m e n t P t r env ( new TurtleSimEnv ( nh ,1) ) ;
19 TaskServer ts ( env ) ;
20 ros :: spin () ;
21 return 0;
22 }

3.4 Task Idle

When no task is required to run, the task server must ensure a well-defined behaviour of the
system. To this end, the task server is instantiated with an Idle task. The default one, which is
instantiated in the above example, just does nothing forever. When writing the Idle class, one has
to be careful that it may be run between two tasks: the server has no way to know that another
task will be required when a specific task terminates. As an example, on an industrial vehicle,
it could make sense for the Idle task to engage the parking brakes, or for an underwater vehicle
to start coming back to the surface if no task is required. In both cases, it would probably be
safer to wait for a small but relevant duration before acting. This would give time to the mission
executive to request the execution of a new task.

3.5 An example: Task GoTo

The purpose of this task is mostly to have the simulated turtle reach a given destination. In
addition to the goal parameters, it will have parameters for its control law (gains, saturation) and
for its completion condition (distance from the goal).
The config file, header and source for this task have been used for the examples above. We
nonetheless copy them here for completeness. First the config file that will define the different
task parameters.
1 # ! / usr / bin / env python
2 PACKAGE = ’ t a s k _ m a n a g e r _ t u r t l e s i m ’
3 import roslib ; roslib . load_manifest ( PACKAGE )
4
5 from d y n a m i c _ r e c o n f i g u r e . p a r a m e t e r _ g e n e r a t o r import *
6 from t a s k _ m a n a g e r _ l i b . p a r a m e t e r _ g e n e r a t o r import *
7
8 gen = T a s k P a r a m e t e r G e n e r a t o r ()
9 # Name Type Description Default Min
Max

8
10 gen . add ( " goal_x " , double_t , 0, " X coordinate of destination " , 0.)
11 gen . add ( " goal_y " , double_t , 0, " Y coordinate of destination " , 0.)
12 gen . add ( " k_v " , double_t , 0, " Gain for velocity control " , 1.0)
13 gen . add ( " k_alpha " , double_t , 0, " Gain for angular control " , 1.0)
14 gen . add ( " max_velocity " , double_t , 0, " Max allowed velocity " , 1.0)
15 gen . add ( " d ist_thre shold " , double_t , 0, " Distance at which a the target is considered
reached " , 0.1)
16
17 exit ( gen . generate ( PACKAGE , " t a s k _ m a n a g e r _ t u r t l e s i m " , " TaskGoTo " ) )

The following headers defines the task definition and instance.

1 # ifndef TASK_GOTO_H
2 # define TASK_GOTO_H
3
4 # include " t a s k _ m a n a g e r _ l i b / Ta skDefin ition . h "
5 # include " t a s k _ m a n a g e r _ t u r t l e s i m / TurtleSimEnv . h "
6 # include " t a s k _ m a n a g e r _ t u r t l e s i m / TaskGoT oConfig . h "
7
8 using namespace t a s k _m a n a g e r _ l i b ;
9
10 namespace t a s k _ m a n a g e r _ t u r t l e s i m {
11 class TaskGoTo : public TaskInstance < TaskGoToConfig , TurtleSimEnv >
12 {
13 public :
14 TaskGoTo ( T a s k D e f i n i t i on P t r def , T a s k E n v i r o n m e n t P t r env ) : Parent ( def , env ) {}
15 virtual ~ TaskGoTo () {};
16
17 virtual TaskIndicator initialise () ;
18
19 virtual TaskIndicator iterate () ;
20
21 virtual TaskIndicator terminate () ;
22 };
23 class Ta sk F ac to ry G oT o : public TaskDefinition < TaskGoToConfig , TurtleSimEnv , TaskGoTo >
24 {
25 public :
26 T as kF ac t or yG oT o ( T a s k E nv i r o n m e n t P t r env ) :
27 Parent ( " GoTo " ," Reach a desired destination " , true , env ) {}
28 virtual ~ T as kF ac t or yG oT o () {};
29 };
30 };
31 # endif // TASK_GOTO_H

An important point to notice is the Boolean true in the TaskFactoryGoTo constructor. This
declares the class as periodic and instructs the scheduler to take care of calling the iterate function
at an approximately constant rate. This rate is one of the default task parameters and a default
value is provided to the task server on initialization.
The implementation of the task follows. Note that it can focus on the mathematics of the task
and does not need to be aware of any infrastructure such as threading, mutexes, data storage and
callbacks, etc...
Another important remark is that the initialise function is unnecessary in this case and could
have been omitted in this code. It is only included here for illustration.
1 # include < math .h >
2 # include " TaskGoTo . h "
3 # include " t a s k _ m a n a g e r _ t u r t l e s i m / TaskGoT oConfig . h "
4 using namespace t a s k _ m a n a g e r _ m s g s ;
5 using namespace t a s k_ m a n a g e r _ l i b ;
6 using namespace t a s k _ m a n a g e r _ t u r t l e s i m ;
7
8 TaskIndicator TaskGoTo :: initialise () {
9 ROS_INFO ( " TaskGoTo : Going to (%.2 f ,%.2 f ) " , cfg . goal_x , cfg . goal_y ) ;
10 return TaskStatus :: T A S K _ I N I T I A L I S E D ;
11 }
12
13 TaskIndicator TaskGoTo :: iterate ()
14 {
15 const turtlesim :: Pose & tpose = env - > getPose () ;
16 double r = hypot ( cfg . goal_y - tpose .y , cfg . goal_x - tpose . x ) ;
17 if ( r < cfg . dist_ thresho ld ) {
18 return TaskStatus :: TASK_CO MPLETED ;
19 }
20 double alpha = remainder ( atan2 (( cfg . goal_y - tpose . y ) , cfg . goal_x - tpose . x ) - tpose . theta ,2*
M_PI ) ;
21 if ( fabs ( alpha ) > M_PI /6) {

9
22 double rot = (( alpha >0) ?+1: -1) * M_PI /6;
23 env - > p ub li sh V el oc it y (0 , rot ) ;
24 } else {
25 double vel = cfg . k_v * r ;
26 double rot = cfg . k_alpha * alpha ;
27 if ( vel > cfg . max_velocity ) vel = cfg . max_velocity ;
28 env - > p ub li sh V el oc it y ( vel , rot ) ;
29 }
30 return TaskStatus :: TASK_RUNNING ;
31 }
32
33 TaskIndicator TaskGoTo :: terminate ()
34 {
35 env - > p ub li sh V el oc i ty (0 ,0) ;
36 return TaskStatus :: T AS K_ TE R MI NATE D ;
37 }
38
39 // Declare the task so that it can be dynamically loaded from the shared library .
40 DYNAMIC_TASK ( T a sk Fa ct o ry Go To ) ;

3.6 Implementation Constraints

3.6.1 Constructors
For a given task, the task instance and task definition constructor must respect the profile used in
the above example and initialize their parent class properly by transferring them the environment
pointer. It is not possible to add other arguments to these constructors since they will always be
called by the task scheduler which knows nothing about additional arguments.
The class inheriting from TaskDefinition (task factory) is the one responsible for naming the
task and specifying its help string. The name must be specified and unique for a given application.
The task scheduler will output a warning when trying to register twice a task with the same name.
The help string could be ignored, but it comes in handy when running a task from the command
line.

3.6.2 Initialization
Variable initialization should be implemented in the initialise function. In the context of a
task class inheriting from the templated TaskInstance, the variable cfg has already been affected
with the values read from the task parameters. The cfg variable is an instance of the Config class
given as a template parameter. The env variable points towards the task environment and could
be used, for instance, to recover a global ROS NodeHandle.
For classes inheriting directly from TaskInstanceBase (not recommended), the parseParameters
function should be overloaded to recover the task parameters and process them as required.
The initialise function is the right place to create ROS publishers or subscribers, and to
affect initial variable values. In particular, any task intending to implement a relative motion, e.g.
rotate over 360 degrees, should use this function to record the current system state.
There are no constraints regarding the duration of the initialise function, it could include
blocking calls or return immediately. However, the task scheduler does not publish the updated
task status while a task is initializing.
The initialise function should return TaskStatus::TASK INITIALISED on success, and
TaskStatus::TASK INITIALISATION FAILED otherwise. Using the setStatusString function al-
lows setting an error status that will then be published by the task scheduler. If a task does not
report itself as initialised, the iterate and terminate functions are not executed.
If a task has no need for initialisation, it should just not overload the initialise function
and use the default one that just returns TaskStatus::TASK INITIALISED

3.6.3 Iterations
The iterate function is, by default, the place to implement the control-loop aspect of a task as
well as its termination conditions. For classes inheriting from TaskInstance, the cfg variable
contains the task parameters and the env variable points towards the task environment.

10
 There are two types of tasks, and they condition the way the iterate function should behave.
Periodic tasks are used when the iterate function should be called at a constant frequency
(up to the OS scheduler precision). In this case, the iterate function should be relatively
short. If the task is deemed completed, the function returns TaskStatus::TASK COMPLETED,
otherwise, it returns TaskStatus::TASK RUN. In the former case, the iterate function is
not called anymore and the task transitions to termination. Example of this type of task
include control loops and tasks waiting for specific events to happen.
Non-periodic tasks are called once and take an undefined time to complete. They update their
status as they see fit and return TaskStatus::TASK COMPLETED when done. Examples of
this type of tasks include path planning functions, service calls, large pre-processing tasks,
etc...
The selection between one type of task and the other is made in the TaskDefinition constructor,
with its third argument (is periodic). A value of true marks the task as periodic.
The iterate function must be overloaded for a task class to be valid (the parent function is
pure virtual).

3.6.4 Termination
The terminate function is called once when the task is completed, whether it completes success-
fully or not. Its role is to leave the system in a consistent and safe state. For instance, on some
platforms, it might be a good habit to set the vehicle speed to zero (assuming it is not flying)
when completing any task.
There are no constraints on the duration of the terminate function. If a class does not require
any specific code on termination, there is no need to overload the parent function.
The terminate function usually returns TaskStatus::TASK TERMINATED. If it needs to report
a failure, it can return TaskStatus::TASK FAILED, but this is unusual at this stage.
The task instance object will be destroyed once a task has terminated, which will close any
publisher or subscriber it owns. This destruction occurs a few seconds after the termination class,
to ensure the task status is correctly updated and published. One should not rely on the instance
destruction to implement system related clean-up. As a result, the task instance destructor is
empty most of the time.

3.6.5 Dynamic Loading

It is assumed that tasks will be loaded dynamically by the task server. To this end, they have to
be compiled as dynamic libraries (or plugins), and they have to define a common handle that the
task scheduler will use to instantiate the task definition.
The DYNAMIC TASK macro creates the required code and should be included in any task intended
for dynamic loading (default behaviour).

3.6.6 Dynamic reconfiguration

When a task instance is created, it launches a ROS dynamic reconfigure server, even before call-
ing the initialise function. Dynamic reconfigure allows the modification of the task parameters
at run-time using a practical graphical user interface. Besides introspection, this function is one
of the reasons why the Config files are used to define task parameters.
This use of dynamic reconfigure has two caveats:
1. First, one should not assume that the value in the cfg variable will stay constant over the
task life. For most parameters (control gains, set-points), this is not an issue. However, some
parameters are used during initialisation to allocate memory or computational structures.
In this case, it is recommended to store the initial value and ignore the cfg value later. Even
better, issue a ROS WARN if the value gets modified to prevent giving the impression that
the reconfiguration server is not working.

11
 2. Second, task parameters have to be single values: float, booleans, strings and integer. Dy-
namic reconfigure does not offer a way to encode arrays in parameters.

3.6.7 Beyond basic argument types

As mentioned above, the normal arguments of a task have to be specified within the dynamic
reconfigure framework, and are then limited to basic types. It is sometimes necessary to pass a
more complex argument to a task3 . To this end, tasks can take an additional argument argv which
can be any ROS message. Note that this message will not be visible to the dynamic reconfigure
framework. Using this functionality requires an action in the mission script and on the task
instance implementation. Here is an example, first in the mission:
1 h = Header ()
2 h . frame_id = " Hello "
3 tc . Clear ( argv = h )

And in the task instance:

1 TaskIndicator TaskClear :: iterate ()
2 std_msgs :: Header header ;
3 if ( e x t r a c t E n c a p s u l a t e d M e s s a g e ( header ) ) {
4 ROS_INFO ( " Clear : got an extra header , frame_id = % s " , header . frame_id . c_str () ) ;
5 }
6 env - > clear () ;
7 return TaskStatus :: TASK_COM PLETED ;
8 }

Note that, even if any message type will work, the type of the message needs to be consistent
between the content of the argv object in python and the extractEncapsulatedMessage function
call in C++, otherwise, the function will fail and return false.

4 Using the task framework

4.1 Compilation and linking
The task framework is dependent on the ROS application building framework. It uses catkin
(even though traces of rosbuild can still be found) and it is governed by a standard catkin
CMakeLists.txt file.
The task framework requires the following elements of the CMakeLists.txt:
• Dependency on package task manager lib and task manager msgs.

• Processing of the config files for each task with the generate dynamic reconfigure options.
See the dynamic reconfigure package documentation for details.
• Compile and link the task server and its environment (assumed to be in src/Environment.cpp):

1 ADD_ EXECUTA BLE ( task_server src / task_server . cpp src / Environment . cpp )
2 T A R G E T _ L I N K _ L I B R A R I E S ( task_server $ { c a t k i n _ L I BR A R I E S } dl )

• Making sure that the tasks plugins are generated in a well-defined place that we can link
from the package launch files:
1 set ( C M A K E _ L I B R A R Y _ O U T P U T _ D I R E C T O R Y
2 $ { C A T K I N _ D E V E L _ P R E F I X }/ $ { C A T K I N _ P A C K A G E _ S H A R E _ D E S T I N A T I O N }/ tasks )

• Adding the tasks as shared library with the following code:

1 ADD_LIBRARY ( TaskName SHARED tasks / TaskName . cpp )
2 T A R G E T _ L I N K _ L I B R A R I E S ( TaskName $ { c a t k i n _ L I B R A R I E S } dl )
3 ADD _ D E P E N D E N C I E S ( TaskName $ { $ { PROJECT_NAME } _ E X P O R T E D _ T A R G E T S })

3 In practice, I could always avoid it before implementing this function

12
 This assumes that a source file TaskName.cpp implements the task classes in the tasks
directory. The ADD DEPENDENCIES is important to inform CMake that the task actually
cares about the generation of the config classes.

4.2 Launch file

The tasks and task server having been compiled by the previously listed CMakeLists.txt, the
task server can be launched with the following command. Note that the lib path parameter is
absolutely necessary to let the task server know where to look for task plugins. With the task
1 <? xml version = " 1.0 " encoding = " UTF -8 " ? >
2 < launch >
3 < node name = " task_server " pkg = " my _ ta sk _ pa ck ag e " type = " task_server " output = " screen " >
4 < param name = " lib_path " value = " $( find my _t as k _p ac ka g e ) / tasks " / >
5 </ node >
6 </ launch >

In case a task is removed from the list of tasks, one will need to be careful to delete the corre-
sponding dynamic library by hand. Otherwise, the task server will keep trying to load it. This
should not have any detrimental effect so long as the old class is not instantiated. If it is, it might
trigger a segmentation fault if the environment or configuration definition changed since it was
compiled.

4.3 Console
The console application is a small layer over the Python client to the task server, instantiated
through ipython. It implements a command line application with the following functionalities:
• List the existing tasks (index()).
• Display current task status (status()).
• Get the help string for the tasks and its parameters (help(task name)).

• Run a task. For instance, for the task GoTo described above, one can use console to run:
GoTo(goal x=5.0,goal y=5.0).
To launch the console, assuming the package task manager lib is in the current ROS workspace,
one can use:

rosrun task manager lib console -s /server node

where server node is the name of the ros node instantiating the task server.

4.4 Simple missions

Simple missions also use the Python client to the task server and ressemble the following script:
1 # !/ usr / bin / python
2 import roslib ; roslib . load_manifest ( ’ t a s k _ m a n a g e r _ t u r t l e s i m ’)
3 import rospy
4 from t a s k _ m a n a g e r _ l i b . TaskClient import *
5
6 rospy . init_node ( ’ task_client ’)
7 tc = TaskClient ( " / task_server " ,0.2)
8
9 while True :
10 tc . Wait ( duration =1.)
11 tc . GoTo ( goal_x =1.0 , goal_y =1.0)
12 tc . Wait ( duration =2.)
13 tc . GoTo ( goal_x =5.0 , goal_y =5.0)

Up to the creation of the tc variable, missions are simple ROS nodes implemented in python. The
task client class is instantiated in the variable tc and requires a first argument which is the name
of the node implementing the task server and the default control loop frequency (0.2s or 5Hz in

13
this example). Based on this information the task client connects to the task server, gets the list
of tasks, and the descriptions of their parameters. It then creates virtual member functions with
the name of the tasks with parameters passed as dictionaries.
Because the task client gets the task list from the server, there is no need to specialize it when
implementing a new task framework. There is also no need to change anything but the mission
when a new task has been added to the server.
An important advantage of using Python to define missions is the possibility to use the normal
Python control flow in the mission definition (here a while statement). Furthermore, because a
mission is a normal ROS node, one can add subscribers to the mission script and possibly take
mission decision based on variables received over ROS.
If a task fails while being executed (real failure, time out, ...), a TaskException will be gener-
ated. It can be caught within the mission script with a standard try ... catch statement and
acted upon as appropriate.
1 try :
2 tc . Wait ( duration =0.2)
3 tc . GoTo ( goal = Andromeda )
4 except TaskException , e :
5 # This means a task failed . We need to react to it
6 rospy . loginfo ( " Task fail with error status % d : % s " \
7 %( e . status , str ( e ) ) )

4.5 Background tasks

In some situations, it may be useful to start a task in the background while another one is running.
It may also be necessary to start two tasks simultaneously and wait for both of them to complete.
Obviously, this only makes sense if the two tasks do not control the same actuators.
By default, tasks are started in the foreground. Only a single task can be run in the foreground,
so when a foreground task starts, it first terminates any existing foreground task (typically the
Idle task).
To start a task in the background, one just needs to add an argument foreground=False to
its parameter list. In this case, the task will be run in its own thread and will not be killed by
starting a foreground task concurrently. The function call then returns the task id:
1 id = tc . WaitForROI ( foreground = False , roi_x =1. , roi_y =6. , roi_radius =1.0)

To wait for a background task to complete, the task client class provides several helper function:
• tc.waitTask(id) wait for the completion of a single task.
• tc.waitAnyTasks([id1,id2,...]) wait for the completion of at least one task within the
provided list.
• tc.waitAllTasks([id1,id2,...]) wait for the completion of all the tasks within the
provided list.
A background task can be terminated with tc.stopTask(id) and tc.stopAllTasks() termi-
nates all tasks currently running.

4.6 Interruptions
There are often well-defined situations where a mission should be interrupted. This is particularly
true in monitoring or service missions where low battery or an unexpected event require aborting
the routine mission and starting a specific action.
The task framework defines these events as Condition variable. Such a Condition class must
have a member function names isVerified(). Currently, the most useful condition is ConditionIsCompleted,
which is True when the task it monitors is completed (or terminated). To add such a condition,
the following syntax is available:
1 id = tc . WaitForROI ( foreground = False , roi_x =1. , roi_y =6. , roi_radius =1.0)
2 tc . addCondition ( C o n d i t i o n I s C o m p l e t e d ( " ROI detector " ,tc , id ) )

14
With such a condition, the mission can be written within a try-catch statement. On the condition
being verified, a TaskConditionException is raised.
1 try :
2 tc . Wait ( duration =0.2)
3 tc . GoTo ( goal_x =0.0 , goal_y =1.0)
4 # Clear the conditions if we reach this point
5 tc . c l ea rC o nd it io n s ()
6 tc . stopTask ( id )
7 except TaskConditionException , e :
8 # This means the conditions were triggered . We need to react to it
9 # Conditions are cleared on trigger
10 DoSomething ()

Note that if the code reaches a point where a specific condition is not required anymore, it should
use the tc.clearConditions() function to remove the current condition set. If the condition
was waiting on the completion of a specific task, then this one must still be running and should
probably be terminated before moving further.

5 Integration with other frameworks

5.1 Integration with Smach
Smach4 is a Python framework within ROS to create complex state machines. In comparison with
the missions written with the task manager, a Smach state machine lives completely within the
Python scripts it is instantiated from. Each state is a Python class and Smach provides a nice
framework to define potential transitions based on the outcome of a state. Smach also provides
a graphical visualisation tool that displays the full state machine, its transitions and the state
currently active. All this allows combining behaviours in a less linear and more complex way.
To take advantage of the strength of Smach while keeping our tasks as well defined behaviours,
a wrapper class MissionStateMachine is provided in the TaskSmach module. This class provides
three main functions to create containers (check Smach documentation for details on the type of
containers):
• createStateMachine(): creates a state machine with parameters compatible with the task
framework.
• createSequence(): create a container for states to be executed as a sequence, which is the
most common setup for a mission.
• createConcurrence(fg task): create a container for at least two states (or nested state-
machines) to be executed concurrently. The concurrence terminates when one of the states
or nested state-machines terminates.

It also provides three functions to create tasks as state-machine states:

• task(name, **params): create a generic state to be added to the state machine.
• seq task(name, ** params): create a state to be inserted in a sequence.
• concurrent task(name, ** params): create a state to be inserted in a concurrence.

• epsilon task(label,transitions): create a state that does nothing but can be used as a
common branching points for real state.
Finally, the MissionStateMachine provides a run function that also instantiates the introspection
server and handles ROS shutdown in a clean way.
An example of Smach based mission can be seen below:
4 http://wiki.ros.org/smach/Documentation

15
 1 # !/ usr / bin / python
2 # ROS specific imports
3 import roslib ; roslib . load_manifest ( ’ t a s k _ m a n a g e r _ t u r t l e s i m ’)
4 import rospy
5 from math import *
6 from t a s k _ m a n a g e r _ l i b . TaskSmach import *
7
8 rospy . init_node ( ’ task_client ’)
9 # Create a SMACH state machine
10 mi = M i s s i o n S t a t e M a c h i n e ()
11 sm = mi . createS equence ()
12
13 # Add states to the container
14 with sm :
15 # Create the initial state , and keep its id .
16 init = mi . seq_task ( " Wait " , duration =1.0)
17 mi . seq_task ( " GoTo " , goal_x =1.0 , goal_y =1.0)
18
19 # Create a concurrence state to handle background tasks
20 sm_con = mi . c r e a t e C o n c u r r e n c e ( ’ normal_seq ’)
21 with sm_con :
22 # First background task
23 mi . c o nc ur r en t_ ta s k ( " WaitForROI " , foreground = False , roi_x =9. , roi_y =6. , roi_radius =1.0)
24 # The second concurrent state is actually a sequence
25 sm_sub = mi . cr eateSeq uence ()
26 with sm_sub :
27 # Execute the three following task in sequence ( assumes p is defined )
28 mi . seq_task ( " Wait " , duration =0.2)
29 mi . seq_task ( " GoTo " , goal_x = p [0] , goal_y = p [1])
30 # Add the sequence to the concurrence
31 smach . Concurrence . add ( ’ normal_seq ’ , sm_sub )
32
33 smach . Sequence . add ( ’ Concurrence ’ , sm_con )
34 # Add the final task , and force it to transition to the initial state .
35 mi . seq_task ( " GoTo " , goal_x =5.0 , goal_y =5.0 , transitions ={ ’ T ASK_COM PLETED ’: init })
36
37 mi . run ( sm )

The resulting state machine can be visualized with smach viewer:

5.2 Integration with the ActionLib

The ActionLib5 is a ROS framework that provides a way to execute long actions, monitor their
execution status and interrupt them. As such, its semantic integrates extremely well with the task
framework presented here. It does not, however, provide a way to sequence different actions in an
easy to read manner.
An action is described by a set of 3 messages, declared in an .action file: a goal, a feedback
and a result. In the context of the task manager, we are mostly concerned with setting the action
5 http://wiki.ros.org/actionlib

16
goal from the task arguments. The feedback is used to check if the corresponding task can report
itself completed and the result is left to the responsibility of the user task.
The integration between the task manager and the ROS ActionLib is implemented in the
task manager action package in the TaskActionGeneric templated class defined in the epony-
mous header. This class is templated on the action type, the task configuration and the task
manager environment as any other task instance. For a specific implementation, the user will
inherit from TaskActionGeneric and specify the following functions:
getActionName(): should provide the name of the action server (see the ActionLib documenta-
tion).
buildActionGoal(Goal & goal): fill in the goal object based on the task configuration variable
in cfg.
handleResult(ResultConstPtr result): maybe overloaded if the task result need to be stored
or acted upon in some way.
As an example, the MoveBase integration described below uses the ActionLib and integrates
with it through the TaskActionGeneric class:
1 template < class TaskEnvironment >
2 class T a s k A c t i o n M o v e B a s e : public TaskActionGeneric < mov e_base_m sgs :: MoveBaseAction ,
TaskActionMoveBaseConfig , TaskEnvironment >
3 {
4 protected :
5 // Create an alias to the Parent class , which must be explicitly used
6 // in this templated inheritance .
7 typedef TaskActionGeneric < mov e_base_ msgs :: MoveBaseAction ,
8 TaskActionMoveBaseConfig , TaskEnvironment > Parent ;
9
10 // In this case the name of the action server is stored in the task
11 // cfg variable .
12 const std :: string & getActionName () const {
13 return Parent :: cfg . action_name ;
14 }
15
16 // Convert the goal from the cfg variable to an ActionLib goal .
17 void b ui ld A ct io nG o al ( typename Parent :: Goal & goal ) const {
18 const T a s k A c t i o n M o v e B a s e C o n f i g & cfg_ = Parent :: cfg ;
19 goal . target_pose . header . frame_id = cfg_ . frame_id ;
20 goal . target_pose . header . stamp = ros :: Time :: now () ;
21
22 goal . target_pose . pose . position . x = cfg_ . goal_x ;
23 goal . target_pose . pose . position . y = cfg_ . goal_y ;
24 goal . target_pose . pose . position . z = cfg_ . goal_z ;
25 goal . target_pose . pose . orientation =
26 tf :: c r e a t e Q u a t e r n i o n M s g F r o m R o l l P i t c h Y a w ( cfg_ . goal_roll ,
27 cfg_ . goal_pitch , cfg_ . goal_yaw ) ;
28 }
29
30 // No result to handle here .
31 public :
32 T a s k A c t i o n M o v e B a s e ( t a sk _ m a n a g e r _ l i b :: T a s k D e f i n i t i o n P t r def ,
33 t a s k _ m a n a g e r _ l i b :: T a s k E n v i r o n m e n t P t r env ) : Parent ( def , env ) {}
34 virtual ~ T a s k A c t i o n M o v e B a s e () {};
35 // Nothing more required , the initialize , iterate and terminate
36 // functions are defined by T a s k A c t i o n G e n e r i c .
37 };

5.3 Integration with MoveBase

MoveBase6 is a ROS framework to handle the navigation of a mobile robot to a goal destina-
tion defined as a ROS pose. It proposes two interfaces: a simple goal which is received on a
ROS topic and is reached without providing explicit feedback and an ActionLib interface. The
two interfaces are proposed in the tasks TaskActionMoveBase and TaskActionMoveGoal in the
task manager action package. Check the task manager turtlesim package for implementation
examples, even though MoveBase is not implemented for the turtlesim.
6 http://wiki.ros.org/move_base

17
 The TaskActionMoveBase uses the ActionLib as defined above. It proposes a generic config
variable with a 6 DoF goal in a specified frame and an action name. Because the TaskActionMoveBase
implements all the interaction with the action server, the instantiation of this task requires simply
to pass the relevant TaskEnvironment. Here is an example from the task manager turtlesim
package:
1 namespace t a s k _ m a n a g e r _ t u r t l e s i m {
2 class TaskMoveBase : public TaskActionMoveBase < TurtleSimEnv >
3 {
4 TaskMoveBase ( T a s k D e f i ni t i o n P t r def , T a s k E n v i r o n m e n t P t r env ) :
5 TaskActionMoveBase < TurtleSimEnv >( def , env ) {}
6 virtual ~ TaskMoveBase () {};
7 };
8
9 class T a s k F a c t o r y M o v e B a s e : public TaskFactoryActionMoveBase < TurtleSimEnv >
10 {
11
12 public :
13 T a s k F a c t o r y M o v e B a s e ( T a s k E n v i r o n m e n t P t r env ) :
14 TaskFactoryActionMoveBase < TurtleSimEnv >( env ) {}
15 virtual ~ T a s k F a c t o r y M o v e B a s e () {};
16 };
17 };

The TaskActionMoveGoal uses the simple goal of the MoveBase framework but requires a
much more complex config structure to achieve the same result. In its simplest form, the task
just send the goal once and report itself completed. If wait completion is set, the task uses
a transform listener (tf package) to monitor the current system configuration and reports itself
complete when the error between the target goal and the current state comes below a threshold.
Hence, the config variables contains the linear and angular thresholds, as well as the reference
frame and vehicle frame to monitor. If necessary, the goal can also be resent regularly either to
trigger a replanning or to handle unreliable connections. Despite the implementation difference,
the task instantiation is very similar to the TaskActionMoveBase class. Here is an example from
the task manager turtlesim package:
1 namespace t a s k _ m a n a g e r _ t u r t l e s i m {
2 class TaskMoveGoal : public TaskActionMoveGoal < TurtleSimEnv >
3 {
4 TaskMoveGoal ( T a s k D e f i ni t i o n P t r def , T a s k E n v i r o n m e n t P t r env ) :
5 TaskActionMoveGoal < TurtleSimEnv >( def , env ) {}
6 virtual ~ TaskMoveGoal () {};
7 };
8
9 class T a s k F a c t o r y M o v e G o a l : public TaskFactoryActionMoveGoal < TurtleSimEnv >
10 {
11
12 public :
13 T a s k F a c t o r y M o v e G o a l ( T a s k E n v i r o n m e n t P t r env ) :
14 TaskFactoryActionMoveGoal < TurtleSimEnv >( env ) {}
15 virtual ~ T a s k F a c t o r y M o v e G o a l () {};
16 };
17 };

5.4 Tasks and multi-agent synchronization

In this section, we assume that multiple agents, each running their local roscore, are linked through
the ros multimaster framework over a non-reliable communication medium, e.g. Wifi. More
information about the multimaster concept can be found on http://wiki.ros.org/multimaster_
fkie.
In this context, each agent may have it own task server and its own tasks and we may need
to create synchronization checkpoints between their missions. This is specifically the objective
of the task manager sync package. Note though that we are not intending to allocate tasks to
a multi-agent system. An example of the kind of mission we are considering would consist of
one loading robot and a delivery robot. The loading robot needs to wait for the delivery robot
to be stopped at the loading dock before loading it, and the delivery truck needs to wait to be
loaded before starting its delivery. Apart from these actions, both robot can run their missions
independently.

18
 An important assumption is that communication is not reliable and that, as a result, each
robot is running his own roscore. The multimaster framework will synchronize topics when the
two robots are connected on the same wifi, but they may not be connected all the time, and it
cannot be assumed that standard ROS message will be delivered. This leads to a design based on
hand-shaking, which unfortunately can be rather verbose. In such a system, any synchronization
message needs to be repeatedly sent until acknowledged, without any guarantees that any of the
messages will be delivered.
The task manager sync package adds two main elements to the normal task server and envi-
ronment. Within the TaskEnvironmentSync parent class, every task servers is given a name (as
a string) and publishes an integer state variable at a constant frequency while listening for the
equivalent variables published by the other task servers on the other agents. Two tasks are also
added automatically to the task server:
• SetStatusSync(status) can be used by one agent to change the value of its own state and
set it to status.
• WaitForStatusSync(partner,status) waits for the partner identified by partner to reach
state status.
Note that this design guarantees that the synchronization point can be reached if at least one
message is delivered. However, it does not prevent deadlocks and it does not guarantee that a
state will be ”seen” by the synchronised agents if it is published for too short a time. As a result,
it is left to the mission designer to make sure that state will only change when acknowledged.
For instance, the delivery truck docks, set its state to ”docked” and wait for the loading truck to
switch to state ”loading finished”. At the same time, the loading truck wait for the delivery truck
to reach the ”docked state”, loads it and then set its state to ”loading finished”. In this case,
string state are used to simplify the semantic, but integers are used in practice. The mapping
between integer and semantic is left to the mission designer as well.

5.4.1 Implementation: task server and environment

Similarly to the other section, an implementation example for the turtlesim is provided in the
task manager turtlesim sync package, which is designed to control three turtles in the simula-
tion window.
Regarding the environment, the only required change is to inherit from the TaskEnvironmentSync
class, and to provide a name to this instance of the task server:
1 // File : t a s k _ m a n a g e r _ t u r t l e s i m _ s y n c / TurtleSimEnv . h
2 # ifndef T U R T L E _ S I M _ E N V _ H
3 # define T U R T L E _ S I M _ E N V _ H
4
5 # include " t a s k _ m a n a g e r _ s y n c / T a s k E n v i r o n m e n t S y n c . h "
6
7 namespace t a s k _ m a n a g e r _ t u r t l e s i m _ s y n c {
8 class TurtleSimEnv : public t a s k _ m a n a g e r _ s y n c :: T a s k E n v i r o n m e n t S y n c
9 {
10 protected :
11 // Same code as the original TurtleSimEnv
12
13 public :
14 TurtleSimEnv ( ros :: NodeHandle & nh , const std :: string & name ) ;
15 ~ TurtleSimEnv () {};
16
17 // Same code as the original TurtleSimEnv
18 };
19 typedef boost :: shared_ptr < TurtleSimEnv > T ur tl eS i mE nv P tr ;
20 typedef boost :: shared_ptr < TurtleSimEnv const > T u r t l e S i m E n v C o n s t P t r ;
21 };
22
23 # endif // T U R T L E _ SI M _ E N V _ H

1 // File : TurtleSimEnv . cpp

2 # include " t a s k _ m a n a g e r _ t u r t l e s i m _ s y n c / TurtleSimEnv . h "
3
4 using namespace t a s k _ m a n a g e r _ t u r t l e s i m _ s y n c ;

19
 5
6 TurtleSimEnv :: TurtleSimEnv ( ros :: NodeHandle & n , const std :: string & name ) :
7 t a s k _ m a n a g e r _ s y n c :: T a s k E n v i r o n m e n t S y n c (n , name , " sync " ) { // " sync " is the topic name
for the state publisher
8 ...
9 }

Using this environment, the task server can be instantiated easily:

1 // File : task_server . cpp
2 # include " t a s k _ m a n a g e r _ s y n c / TaskSer verSync . h "
3 # include " t a s k _ m a n a g e r _ t u r t l e s i m _ s y n c / TurtleSimEnv . h "
4
5 using namespace t a s k _ m a n a g e r _ t u r t l e s i m _ s y n c ;
6 using namespace t a s k _m a n a g e r _ l i b ;
7
8 // Everything inherited from TaskServerSyn c
9 class TaskServer : public t a s k _ m a na g e r _ s y n c :: TaskSe rverSyn c {
10 public :
11 TaskServer ( T a s k E n v i r o n m e n t P t r _env ) : t a s k _ m a n a g e r _ s y n c :: Task ServerSy nc ( _env ) {
12 start () ;
13 }
14
15 };
16
17 int main ( int argc , char * argv [])
18 {
19 ros :: init ( argc , argv , " tu rt le s im_t as k s " ) ; // init ros
20 ros :: NodeHandle nh ( " ~ " ) ;
21 std :: string partner_name = " partner " ;
22 nh . getParam ( " my_name " , partner_name ) ;
23
24 // Create an environment for this instance and make sure the partner name
25 // used .
26 Tur tl eS i mE nv P tr env ( new TurtleSimEnv ( nh , partner_name ) ) ;
27 // Add subscription to sync signals from other agents ( which may be
28 // received from the multimaster sync mechanism . The topic names " partner1 "
29 // or " partner2 " are expected to be remapped in the launch file .
30 env - > addSyncSource ( " partner1 " ) ;
31 env - > addSyncSource ( " partner2 " ) ;
32 // And we are done .
33 TaskServer ts ( env ) ;
34 ros :: spin () ;
35 return 0;
36 }

Finally, this task server is instantiated and with the following launch file (without the multi-
master context)
1 <? xml version = " 1.0 " encoding = " UTF -8 " ? >
2 < launch >
3 <! -- Launching the turtlesim node and spawning two additional turtles -- >
4 < node name = " turtlesim " pkg = " turtlesim " type = " tur tlesim_ node " / >
5 < node pkg = " rosservice " type = " rosservice " name = " spawn2 " args = " call -- wait / spawn 4 4 0
turtle2 " / >
6 < node pkg = " rosservice " type = " rosservice " name = " spawn3 " args = " call -- wait / spawn 7 7 0
turtle3 " / >
7
8 <! -- Launching task server for turtle 1 and connecting it to the sync signals from
turtle 2 and 3 -- >
9 < node name = " t u r t l e s i m _ t a s k s 1 " pkg = " t a s k _ m a n a g e r _ t u r t l e s i m _ s y n c " type = "
t u r t l e s i m _ t a s k _ s e r v e r _ s y n c " output = " screen " >
10 < param name = " lib_path " value = " $( find t a s k _ m a n a g e r _ t u r t l e s i m _ s y n c ) / tasks " / >
11 < param name = " my_id " value = " 1 " / >
12 < param name = " my_name " value = " partner1 " / >
13 < remap from = " ~/ partner1 " to = " / t u r t l e s i m _ t a s k s 2 / sync " / >
14 < remap from = " ~/ partner2 " to = " / t u r t l e s i m _ t a s k s 3 / sync " / >
15 </ node >
16
17 <! -- Launching task server for turtle 2 and connecting it to the sync signals from
turtle 1 and 3 -- >
18 < node name = " t u r t l e s i m _ t a s k s 2 " pkg = " t a s k _ m a n a g e r _ t u r t l e s i m _ s y n c " type = "
t u r t l e s i m _ t a s k _ s e r v e r _ s y n c " output = " screen " >
19 < param name = " lib_path " value = " $( find t a s k _ m a n a g e r _ t u r t l e s i m _ s y n c ) / tasks " / >
20 < param name = " my_id " value = " 2 " / >
21 < param name = " my_name " value = " partner2 " / >
22 < remap from = " ~/ partner1 " to = " / t u r t l e s i m _ t a s k s 1 / sync " / >
23 < remap from = " ~/ partner2 " to = " / t u r t l e s i m _ t a s k s 3 / sync " / >
24 </ node >
25

20
26 <! -- Launching task server for turtle 3 and connecting it to the sync signals from
turtle 1 and 2 -- >
27 < node name = " t u r t l e s i m _ t a s k s 3 " pkg = " t a s k _ m a n a g e r _ t u r t l e s i m _ s y n c " type = "
t u r t l e s i m _ t a s k _ s e r v e r _ s y n c " output = " screen " >
28 < param name = " lib_path " value = " $( find t a s k _ m a n a g e r _ t u r t l e s i m _ s y n c ) / tasks " / >
29 < param name = " my_id " value = " 3 " / >
30 < param name = " my_name " value = " partner3 " / >
31 < remap from = " ~/ partner1 " to = " / t u r t l e s i m _ t a s k s 1 / sync " / >
32 < remap from = " ~/ partner2 " to = " / t u r t l e s i m _ t a s k s 2 / sync " / >
33 </ node >
34 </ launch >

5.4.2 Mission examples

The task set used in the task manager turtlesim sync is the same as for the standard task manager turtlesim,
with the addition of the two tasks mentioned above. The resulting sample missions are given be-
low. They result in the 3 turtles doing their own missions but with sync point where they wait
for each other.
• Turtle 1
1 # !/ usr / bin / python
2 # File mission1 . py : mission for turtle 1
3 import roslib ; roslib . load_manifest ( ’ t a s k _ m a n a g e r _ t u r t l e s i m _ s y n c ’)
4 import rospy
5 from t a s k _ m a n a g e r _ l i b . TaskClient import *
6
7 rospy . init_node ( ’ task_client1 ’)
8 # Make sure to select the specific server node
9 server_node = rospy . get_param ( " ~ server " ," / t u r t l e s i m _ t a s k s 1 " )
10 defa ult_per iod = rospy . get_param ( " ~ period " ,0.2)
11 tc = TaskClient ( server_node , defa ult_peri od )
12
13 # reset status
14 tc . SetStatusSync ( status =0) ;
15 # Move to start position and switch to state 1
16 tc . GoTo ( goal_x =1.0 , goal_y =1.0)
17 tc . SetStatusSync ( status =1) ;
18 # Wait for the two other turtles to have completed their tasks and switched to status 1
19 tc . W a i t F o r S t a t u s S y n c ( partner = " partner2 " , status =1) ;
20 tc . W a i t F o r S t a t u s S y n c ( partner = " partner3 " , status =1) ;
21 # Go back to the center and switch back to state 1
22 tc . GoTo ( goal_x =5.0 , goal_y =5.0)
23 tc . SetStatusSync ( status =0) ;
24
25 rospy . loginfo ( " Mission1 completed " )

• Turtle 2
1 # !/ usr / bin / python
2 # File mission2 . py : mission for turtle 2
3 import roslib ; roslib . load_manifest ( ’ t a s k _ m a n a g e r _ t u r t l e s i m _ s y n c ’)
4 import rospy
5 from t a s k _ m a n a g e r _ l i b . TaskClient import *
6
7 rospy . init_node ( ’ task_client2 ’)
8 # Make sure to select the specific server node
9 server_node = rospy . get_param ( " ~ server " ," / t u r t l e s i m _ t a s k s 2 " )
10 defa ult_peri od = rospy . get_param ( " ~ period " ,0.2)
11 tc = TaskClient ( server_node , defa ult_peri od )
12
13 # Reset state
14 tc . SetStatusSync ( status =0) ;
15 # Wait for turtle 1 to switch to state 1
16 tc . W a i t F o r S t a t u s S y n c ( partner = " partner1 " , status =1) ;
17 # Move to specific position and switch this turtle ’s state to 1
18 tc . GoTo ( goal_x =9.0 , goal_y =1.0)
19 tc . SetStatusSync ( status =1) ;
20 # Wait for turtle 3 to reach 1 as well , and for turtle 1 to come back to zero .
21 tc . W a i t F o r S t a t u s S y n c ( partner = " partner3 " , status =1) ;
22 tc . W a i t F o r S t a t u s S y n c ( partner = " partner1 " , status =0) ;
23 # Now it is time to go home and switch back to 0.
24 tc . GoTo ( goal_x =9.0 , goal_y =9.0)
25 tc . SetStatusSync ( status =0) ;
26
27 rospy . loginfo ( " Mission2 completed " )

21
 • Turtle 3
1 # !/ usr / bin / python
2 # File mission3 . py : mission for turtle 3
3 import roslib ; roslib . load_manifest ( ’ t a s k _ m a n a g e r _ t u r t l e s i m _ s y n c ’)
4 import rospy
5 from t a s k _ m a n a g e r _ l i b . TaskClient import *
6
7 rospy . init_node ( ’ task_client3 ’)
8 # Make sure to select the specific server node
9 server_node = rospy . get_param ( " ~ server " ," / t u r t l e s i m _ t a s k s 3 " )
10 defa ult_per iod = rospy . get_param ( " ~ period " ,0.2)
11 tc = TaskClient ( server_node , defa ult_peri od )
12
13 # Reset state
14 tc . SetStatusSync ( status =0) ;
15 # Wait for turtle 2 to switch to state 1 ( which will happen after turtle 1 does it )
16 tc . W a i t F o r S t a t u s S y n c ( partner = " partner2 " , status =1) ;
17 # Move to specific position and then switch this turtle ’s state to 1
18 tc . GoTo ( goal_x =1.0 , goal_y =9.0)
19 tc . SetStatusSync ( status =1) ;
20 # Wait for the two other turtles to switch back to 0
21 tc . W a i t F o r S t a t u s S y n c ( partner = " partner1 " , status =0) ;
22 tc . W a i t F o r S t a t u s S y n c ( partner = " partner2 " , status =0) ;
23 # Finally go home and switch itself to 0.
24 tc . GoTo ( goal_x =1.0 , goal_y =1.0)
25 tc . SetStatusSync ( status =0) ;
26
27 rospy . loginfo ( " Mission3 completed " )

6 Acknowledgements
Some of the work presented in this document have been supported by EU Grant 644227 “Flourish”,
http://flourish-project.eu/.

22