2018 Latin American Robotic Symposium, 2018 Brazilian Symposium on Robotics (SBR) and 2018 Workshop on

Robotics in Education (WRE)

A ROSPlan-based multi-robot navigation system

Dannilo Samuel Silva Miranda, Luiz Edival de Souza, Guilherme Sousa Bastos
Department of Electrical Engineering, Federal University of Itajubá
Av. BPS, 1303, Bairro Pinheirinho - Itajubá MG, Brazil

Abstract—Multi-robot systems is a growing research field in tools to generate the problem file from the knowledge
robotics and artificial intelligence. Applying multiple robots on parsed, automate calls to the planner, them post-process and
single robot systems, generally increase the execution robust- validate the plan, handle the main dispatch loop and monitor
ness while minimizing its time. Besides this, if the execution
requires joint or cooperative actions there isn’t better option. the execution of the plan. ROSPlan also supports replanning
This paper proposes a multi-robot version of RosPlan, which in any case of failure.
that is a framework for embedding a generic task planner in The introduction of a multi-robot system can facilitate
a ROS system. The main contributions include a new method and increase the possibilities of executing a task, allowing
to dispatch the actions; a system to ensure that each robot plans that can not be executed by just one robot be executed
execute only its actions and a new ROS Package for multi-
robot navigation. by a set of robots with a common goal. Even sharing a
common goal it is necessary to coordinate the dispatch
Keywords-RosPlan; PDDL; Planning; Multi-robot; of actions for each robot, and so that the actions are not
carried out in a disorderly manner and invalidate the plan.
I. I NTRODUCTION
The first contribution of this paper is a new method for
In recent years the number of multi-robot systems de- dispatching actions in a coordinated way. Our approach
ployed in field applications has risen dramatically, replacing allows to dispatch more than one action at time to several
dangerous tasks that were once done only by humans, different robots, being robots able to perform their actions
like fire-fighting [1], landmine detection [2] and underwater in a shared environment without interfere negatively in the
missions [3]. actions of the others.
Control a robot is a complex task and requires a lot In addiction, for this work was develop a navigation
of skills, like determine which robots are better for the package for multi-robot, because although already existent
task and the sequence of high-level tasks that should be navigation packages in ROS and it works with multi-robot,
performed to accomplish the mission. In the case of multi- none of them do multi-robot path planning.
robot, this assignment becomes even more difficult perform
by a human. So, providing the autonomy to the robots or II. R ELATED W ORKS
designating a computer to do the planning and controlling The task planning is essential in solving complex prob-
the execution of the tasks is a better alternative. lems, a good planner have to choose and organize tasks to
Automatic planning is an area within the field of Arti- bring the system from the initial state to a goal. Many au-
ficial Intelligence that can be defined like the process of thors have already verified the effectiveness of task planning
considering and organizing actions to achieve goals, before in applications with robots like [6] that use the planning
starting to execute them [3]. This tool is vital for a robotic to solve problems with aerial missions. In this work was
agent operating in an environment where a chain of events evaluated the expressiveness of the PDDL language to
could lead to a dead-end state. The planning community has describe a scenario with multi-robot in a temporal plan and
standardized STRIPS-like planning with the introduction of it was pointed that this type of planning is able to execute
the Planning Domain Definition Language (PDDL) [4]. In complex time calculations and exploit the parallelism and
this language, a planning problem is formally described by competition of actions automatically.
providing two files: the domain file containing the actions In [7] is discussed the planning problems that can not
with their preconditions and effects and the problem file be solved by single agents and needs the cooperation of
containing the initial state and the goals. several robots to achieve the goal. The experimental results
Combining task planning and robotics present several of this work show that, multi-agent planning problem having
challenges, most of these are addressed by the ROSPlan [5]. a greater number of objects, agents, and location cannot be
ROSPlan is a framework introduced to provide a generic solved by existing planners. Since, these planners assumed
method for planning and executing tasks in a ROS (Robot agents as an object the state space drastically increases for
Operating System) system. It is a link between two standard them. Therefore, they fail after a certain level. Trying to
approaches, PDDL and ROS. Basically, ROSPlan provides work around this problem we propose a planning in two

978-1-5386-7761-2/18/$31.00 ©2018 IEEE 248

DOI 10.1109/LARS/SBR/WRE.2018.00053
stages, first a high level planning for the actions and then a
path planning during the execution of the plan.
In this article, we chose work with the framework ROS-
Plan because this package proved effective in its proposal,
and many researchers are adding their contributions in it,
like the [8], that build a conditional planning for generating
and executing short-term interactions by a robot deployed
in a public environment; the [9], that describe a system for
autonomously searching, detecting, and tracking an object
of interest with a small unmanned aerial vehicle (sUAV);
and the [3], that work with opportunistic planning prob-
lem, in theirs work are explores the execution of planned
autonomous underwater vehicle (AUV) missions where op-
portunities to achieve additional utility can arise during Figure 1. Example of path generated by the planner. The green arrow
execution. indicates the path of the robot 1 and the red arrow the path of the robot 2
A great advantage of ROSPlan is to have been built in
the platform ROS [10] that is an architecture that provides
libraries and tools to help software developers build robotic • “dispatch concurrent && dispatch on completion”:
systems. ROS has become a popular platform for robotics Actions are dispatched all at once;
research and one of its advantages it is licensed under an • “!dispatch concurrent && dispatch on completion”:
open source, and each year the number of contributions Actions are dispatched as soon as the previous action
increases significantly. is completed;
• “!dispatch concurrent && !dispatch on completion”:
III. P LANNING S YSTEM Actions are dispatched at the time they are scheduled in
The task planning will work in two stages. In the first state the plan, or as soon as the previous actions completes;
a classical algorithm of planning will generate the height • “dispatch concurrent && !dispatch on completion”:
level action, such as indicate where the robot should go Actions are dispatched at the time they are scheduled
(end point) or what it should do. This planning happens in the plan.
just before it start executing the planning or when the plan None of these options are desirable for a multi-robot
was invalidated and a replannig was requested. system. The first option sends all the action at the beginning,
When it comes to an action of locomotion, the second without any checking, this situation is almost never wanted.
state of planning happen, from the data provided by the The second option dispatch only one action at a time; in
classical planning, a new algorithm developed to a path plan other words, the second action will be send only when the
is used. This algorithm is responsible for defining each point first one is finished. As result of that a robot cannot execute
that the robot has to pass until it achieves its end point. For multi-action neither joint actions. The third option sends the
example, the Figure 1 shows that the robot 1 is in the point action only when the time in which it was scheduled has
S1 and the robot 2 is in the point S2. The goal is the robot 1 already gone, however it dispatch the next action only if the
is to arrive at G1 and robot 2 at G2. The classical planning previous has already finished. The forth and last one, the
will generate the task to the robot 1 leave from S1 and arrive action can be sent, at the same time, to two robots. In this
in the spot G1 and the robot 2 leave from S2 and arrive in case the actions will be dispatched at the exact time they
the spot G2. The path planning package shows all points were scheduled. The problem with this option is that it is
that the robot must go through to reach the goal avoiding not very robust. If the execution time of any action be longer
collisions. than the scheduled one, the plan will fail requiring then a
IV. D ISPATCH M ETHOD replanning. As well, if the action ends before the planned
time, the robot will not be able to advance to the next action,
After the planner generates the list of actions that have staying paused until the scheduled time for this action runs
to be executed is necessary to coordinate the dispatch of out.
actions for each robot, so that all the actions are executed
The developed method consists in a supervisor make a
without the plan being invalidated and in this way, the
list with actions, in a sequential form, then check if all the
final goal is achieved. ROSPlan offers some strategies to
pre-conditions of the first action are satisfied. In the positive
dispatch the plan, this strategies are based on the ROS
case, this action will be executed and added in another list
launch file parameters, “dispatch-concurrent” and “dispatch-
that represents the execution action list. As soon as the
on-completion”. The behavior of dispatch depends on the
first action is dispatched, the second action check begins.
combination of these values.
Primarily, is verified if comply with all the pre-condition. In

249
a positive case, the supervisor will ascertain that the effects ( a t end ( c l o s e ? d ) ) ) )
of this action will not get away or negate the execution of ( : d u r a t i v e −a c t i o n o p e n d o o r
the others actions that are in the execution list. If any of the : p a r a m e t e r s ( ? v − r o b o t ? d − d o o r ?w ? from
effect actions interfere in which they are being executed, ? to − waypoint )
then this action is also dispatch. This procedure is made : d u r a t i o n (= ? d u r a t i o n 5)
until the all messages are dispatch. The block diagram in : c o n d i t i o n ( and
the Figure 2 illustrates the operation. ( a t s t a r t ( u n l o c k ? d ?w ? from ? t o ) )
( a t s t a r t ( r o b o t a t ? v ?w ) )
( at s t a r t ( close ?d ) ) )
: e f f e c t ( and
( at s t a r t ( not ( close ?d ) ) )
( a t end ( c o n n e c t e d ? from ? t o ) )
( a t end ( c o n n e c t e d ? t o ? from ) )
( a t end ( open ? d ) ) ) )
)
A possible PDDL plan for two robots that can be gener-
ated is:
• (goto waypoint robot1 wp1 wp3)
• (goto waypoint robot2 wp0 wp5)
• (open door robot1 b wp3 wp5 wp8)
• (goto waypoint robot1 wp3 wp6)
• (goto waypoint robot2 wp5 wp8)
• (close door robot1 b wp6 wp5 wp8)
• (goto waypoint robot2 wp8 wp10)

Figure 2. Block diagram of the dispatch method steps In order to dispatch these actions, the supervisor checks
the first message of the list, how all preconditions are
attended and if does not have any action in the executed
For a better comprehension of the work and to exemplify,
list. Then, the supervisor dispatches the action 1 and adds
consider the actions with their preconditions and effects of
it in the execution list. The same is made with the next action
the following domain:
of the planner list; in other words, the preconditions of the
( : d u r a t i v e −a c t i o n g o t o w a y p o i n t action 2 are attended, it starts to verify if any effects of the
: p a r a m e t e r s ( ? v − r o b o t ? from action 2 cancels some precondition of those action in the
? to − waypoint ) list that are executed. If there is not any limitation, action 2
: d u r a t i o n ( = ? d u r a t i o n 10) is dispatched and added in the list of actions that are been
: c o n d i t i o n ( and executed. The dispatch of all the others actions is made in
( a t s t a r t ( r o b o t a t ? v ? from ) ) the same way.
( o v e r a l l ( c o n n e c t e d ? from ? t o ) ) ) When some action is concluded with success, the algo-
: e f f e c t ( and rithm receives a message and take off this action from the
( a t s t a r t ( n o t ( r o b o t a t ? v ? from ) ) ) execution list. Figure 3 represents all the actions previously
( a t end ( r o b o t a t ? v ? t o ) ) listed with the precondition that need to be attended to
( a t end ( v i s i t e d ? t o ) ) ) ) dispatch the next action.
( : d u r a t i v e −a c t i o n c l o s e d o o r Notice that to complete the action 6 is only necessary
: parameters (? v − r o b o t ?d − door that actions 4 and 3 have already been executed, however
?w ? from ? t o − w a y p o i n t ) it cannot be dispatched while the action 5 has not con-
: d u r a t i o n (= ? d u r a t i o n 5) cluded, since the effect (not(connected wp5 wp8)) of the
: c o n d i t i o n ( and action 6 cancels the precondition(connected wp5 wp8) of
( a t s t a r t ( l o c k ? d ?w ? from ? t o ) ) the action 5.
( a t s t a r t ( r o b o t a t ? v ?w ) )
( a t s t a r t ( open ? d ) ) ) V. TASK EXECUTION
: e f f e c t ( and To perform the tasks, each robot is entered in a different
( a t s t a r t ( n o t ( open ? d ) ) ) namespace. The namespace is used to avoid resource conflict
( a t s t a r t ( n o t ( c o n n e c t e d ? from ? t o ) ) ) and to promote a encapsulation, and this is extremely neces-
( a t s t a r t ( n o t ( c o n n e c t e d ? t o ? from ) ) ) sary in an environment with more than one robot. Whereas

250
 robots, in which one have a prediction of collision and that
there is previously generated alternative plans to avoid them.
There are already several works in the literature that
approaches the trajectory coordination of multiple mobile
robots [11]–[14]. The differential of this work is the appli-
cation of a technique of a multiple robot path planning in
the navigation. In addition, new robots can be inserted in the
environment at any moment being a plan created to avoid
collisions with fixed obstacles and robots that are already
executing some task.
The “classical prioritized plannin” [11] is a method widely
used for this type of application. We choose this technique
because of the simplicity of the algorithm that consists in
assigning a different priority for each robot and generate
the plan sequentially. The robot that has the highest priority
has its plan generate first, and the one with lowest priority
are generated in order to avoid collisions with the highest
priority.
In order to avoid collisions with another robots, the
Figure 3. Flow diagram of the dispatch method concept of time-space is used, in which a robot occupies
a certain region during a period of time τ (x, y, t). From a
map ω provided by the planner, the starting points Si and
two robots have a topic for publishing its odometry with the the arrival Gi of a robot. The algorithm must be able to
name “\odom”, if these two robots are integrate in the same return a plan π avoiding the trajectory Δ that the robots with
system, the names conflicts of the topics is avoided by the higher priority will pass. The pseudo code of the Classical
namespace given to each robot. Prioritized Planning is represented in the Algorithm 1.
Every action that the robot can execute was implemented
in a separate node and added to the robot package. All these Algorithm 1 Classical Prioritized Planning
nodes should be subscribe the dispatch topic and have a 1: procedure A LGORITHM PP
mechanism to verify if the published action is intended for 2: Δ←Ø
3: for i ← 1...n do
them. In other words, when any action is published in the 4: πi ← BEST_TRAJ(ω, Δ, Si , Gi )
dispatch topics, all subscribed nodes have to verify what is 5: if πi = Ø then
written in this action. First it is verified if it corresponds 6: report failure and terminate
at robot, then verify which action is involved, and finally 7: Δ ← Δ ∪ πi
if this action is well formed, presenting all the necessary 8: function B EST TRAJ(ω, Δ, Si , Gi )
arguments. Having all these requirements met, the node will 9: return optimal satisfying trajectory for robot i in ω that
10: avoids regions Δ if it exists, otherwise return Ø
be activated and the task executed.
As soon as the robot completes the task execution, a
conclusion massage should be send to the complete topic Some adaptations in the algorithm were made for this
tasks. This massage will update the execution status to work, the planning of the trajectory of each robot is done in
finished task, added the action effects in the knowledge base a central separated node, when some robots receive an action
and will delete the vector actions in executions, as explained that demands a path in the map, it must take a request to
in the previous section. the planner through a ROS SERVICE, which is defined by
a pair of messages: one for the request and other one for
VI. NAVIGATION PACKAGE the reply. The robot calls the service by sending the request
message with its start and goal position waiting the reply.
The ability of a robot to move freely in an environment
The planner will calculate the path avoiding the trajectories
greatly increases the possibilities of tasks that it can per-
that have already been send, add in the vector of trajectory
form. For this, an autonomous navigation system installed
execution Δ and reply the path to the robot. With the planned
is needed, which is responsible to generate the robots
path, the robot begins the navigation using the odometry to
trajectories.
locate its pose. At each subpoint that the robot achieve, it
At the ROS environment there are already some naviga-
publishes in a topic of sub goal achieve that the path planner
tion packages (like move base and mrpt navigation) that are
is subscriber, and the vector of trajectory Δ in execution is
capable to move the robot by the map and deviates it from
updated.
obstacles. However, any of them execute planning for multi-

251
 A simplified block diagram of all step of the planner is
represented in Figure 4. The block 1 is the ROSPlan, where
the task planning is done and the actions list to be executed
is generated. The dispatch of actions is done as explained
above. When a message is sent, all robots receive it, however
only the one for which the message is destined will execute.
If the message is to move in the map, the robot requests the
path to the pathplan, represented by the block 3. As soon as
it receives the response it starts to execute the task.

Figure 5. Initial states of the environment

robot 2. In the yellow point a collision would happen, but it

was identified and the path represent by the green line was
sent to robot 2 because it had lower priority. The simulation
can be watched at the video1 .

Figure 4. Block diagram of system steps

VII. S IMULATION
The environment presented in Figures 5 and Figures 7
were developed in GAZEBO [15]. Each robot was managed
with the same namespace used in the task planner. This way
facilitates the verification that the robots should do when a
message arrives to them. A node with the interpretation of
each action also have to be executed for each robot.
Here we can highlight two main examples, both using the
previously domain shown in “Dispatch Method” section. The Figure 6. Path taken by robots and collision avoided
initial state of the first example is in the Figure 5 and the
goal to be achieved is to have the “robot 2” in the wp10 The second simulation has the intention to check the
and the “door b” closed. ability of the navigation package to avoid collisions in an
As expected, the dispatch of the action happened like environment with multiple robots crossing the map. For this,
the figure 3, and in this example was possible observed a map with seven robots and four corridors was created, the
the capacity of this method generate a plan that can have map and the initial position of the robots is represented in
more them one robot execute actions in the same time and the figure 7. The numbering on each robot represents which
the capacity of one robot execute more them one action robot it is and the order of priority that was used to find the
simultaneously, therefore what will limit how the action will ways, the circle with the numbers represents the destinations
be dispatch and if the robot will be able to execute multi of them.
action is the file domain. As expected, the paths was generated for all robots avoid
Still in this example was possible observe a collision that the collisions already in the planning phase and even without
was avoided. In the path of robot 1 from wp1 to wp3 it any sensors that detect the presence of each robot. This
goes in the same path of robot 2 from wp0 to wp5. The way, was possible that all the robots arrived at their final
first path generated to the robot 1 is represented by the red destination.
line in the Figure 6, the blue line represents the path of the 1 https://www.youtube.com/watch?v=cVRndd8PeQI

252
 [3] M. Cashmore, M. Fox, D. Long, D. Magazzeni, and B. Rid-
der, “Opportunistic planning in autonomous underwater mis-
sions,” IEEE Transactions on Automation Science and Engi-
neering, vol. 15, no. 2, pp. 519–530, 2018.

[4] V. Estivill-Castro and J. Ferrer-Mestres, “Path-finding in

dynamic environments with pddl-planners,” in Advanced
Robotics (ICAR), 2013 16th International Conference on.
IEEE, 2013, pp. 1–7.

[5] M. Cashmore, M. Fox, D. Long, D. Magazzeni, B. Ridder,

A. Carrera, N. Palomeras, N. Hurtós, and M. Carreras,
“Rosplan: Planning in the robot operating system.” in ICAPS,
2015, pp. 333–341.

[6] L. F. Cantoni, M. F. Campos, and L. Chaimowicz,

“Investigação da linguagem pddl no planejamento de
missões para robôs aéreos,” X SBAI–Simpósio Brasileiro de
Automação Inteligente, São João del Rei, Minas Gerais.
Brasil, 2011.

[7] S. S. Chouhan, A. Singh, and R. Niyogi, “Multi-agent

Figure 7. Initial and final state of the second example
planning with joint actions,” in International Conference on
Advances in Computing, Communications and Informatics
(ICACCI). IEEE, 2015, pp. 1284–1290.
VIII. C ONCLUSION
[8] V. Sanelli, M. Cashmore, D. Magazzeni, and L. Ioc-
This paper provided a tool to work with a multi-robot chi, “Short-term human-robot interaction through conditional
system using the ROSPlan package. The dispatch method planning and execution,” International Conference on Auto-
mated Planning and Scheduling (ICAPS).
described here worked well, not needing replanning every
time an action delays and advancing the next actions of [9] R. Morris, A. Chakrabarty, J. Baculi, X. Bouyssounouse, and
the plan when possible. The message verification system R. Hunt, “Autonomous search-detect-track for small uavs,”
ensured that only the robots that the messages were destined IntEx 2017, p. 19, 2017.
performed the action. The navigation system was able to
[10] M. Quigley, K. Conley, B. Gerkey, J. Faust, T. Foote, J. Leibs,
prevent collisions, as result, the robot did not need to arrive
R. Wheeler, and A. Y. Ng, “Ros: an open-source robot op-
in the collision point to generate a new path. erating system,” in ICRA workshop on open source software,
There is still a lot of work to be done, particularly in nav- vol. 3, no. 3.2. Kobe, 2009, p. 5.
igation, since the Priority planning is not always able to find
a valid plan, returning failure and needing a replanning. For [11] M. Čáp, P. Novák, A. Kleiner, and M. Seleckỳ, “Prioritized
planning algorithms for trajectory coordination of multiple
future works we will be study extension of priority planning mobile robots,” IEEE Transactions on Automation Science
to decentralized system, a method to automatically create and Engineering, vol. 12, no. 3, pp. 835–849, 2015.
the navigation map, and use more sensors in localization
and deviation of obstacles. [12] D. Silver, “Cooperative pathfinding.” AIIDE, vol. 1, pp. 117–
122, 2005.
ACKNOWLEDGMENT [13] P. Svestka and M. H. Overmars, “Coordinated motion
The authors of this paper would like to thank Capes for planning for multiple car-like robots using probabilistic
roadmaps,” in Robotics and Automation, 1995. Proceedings.,
the financial support. 1995 IEEE International Conference on, vol. 2. IEEE, 1995,
pp. 1631–1636.
R EFERENCES
[14] C. W. Warren, “Multiple robot path coordination using artifi-
[1] J. Saez-Pons, L. Alboul, J. Penders, and L. Nomdedeu, cial potential fields,” in Robotics and Automation, 1990. Pro-
“Multi-robot team formation control in the guardians project,” ceedings., 1990 IEEE International Conference on. IEEE,
Industrial Robot: An International Journal, vol. 37, no. 4, pp. 1990, pp. 500–505.
372–383, 2010.
[15] N. Koenig and A. Howard, “Design and use paradigms for
[2] R. K. Megalingam, V. Gontu, R. Chanda, P. K. Yadav, and gazebo, an open-source multi-robot simulator,” in Intelligent
A. P. Kumar, “Landmine detection and reporting using light Robots and Systems, 2004.(IROS 2004). Proceedings. 2004
weight zumo bot,” in Inventive Computing and Informatics IEEE/RSJ International Conference on, vol. 3. IEEE, 2004,
(ICICI), International Conference on. IEEE, 2017, pp. 618– pp. 2149–2154.
622.

253