## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note =
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links
## note.0=
## descriptive title for the tutorial
## title = Multi-TIAGo Navigation
## multi-line description to be displayed in search
## description = This tutorial shows how to launch the Multi-TIAGo navigation simulation in Gazebo and how to run autonomous navigation avoiding obstacles by means of global and local path planning.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for
## level= IntermediateCategory
## keywords = Multi-TIAGo, multi-robot navigation
####################################
Author: Alessandro Di Fava

Maintainer: Jordi Pages < jordi.pages@pal-robotics.com >, Alessandro Di Fava < alessandro.difava@pal-robotics.com >

Support: tiago-support@pal-robotics.com

Source: https://github.com/pal-robotics/tiago_simulation.git
<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

{{attachment:multi_rviz_goal2.png||align="right",width="200"}} 
<<TableOfContents(3)>>

== Purpose ==

This tutorial shows how to launch the Multi-TIAGo navigation simulation in the Gazebo environment and how to run autonomous navigation avoiding obstacles by means of global and local path planning.
 
== Pre-requisites ==

First make sure that the tutorials are properly installed along with the TIAGo simulation, as shown in the [[Robots/TIAGo/Tutorials#Tutorials_Installation|Tutorials Installation]] Section. Then, follow the instructions in [[http://wiki.ros.org/Robots/TIAGo/Tutorials/Multi_TIAGo_simulation] tutorial in order to understand how the Multi-TIAGo simulation is launched.

== Execution ==

First open a console and source the public simulation workspace as follows:

{{{
cd ~/tiago_public_ws
source ./devel/setup.bash
}}}

== Launching the simulation ==

In the console launch the following simulation

{{{
roslaunch tiago_multi multitiago_gazebo_navigation.launch
}}}

Gazebo will show up with two TIAGo robots in the PAL office environment.

{{attachment:gazebo_multi.png||align="center", width="600"}}

Wait until the two TIAGo robots have tucked their arm. Then you may proceed with the next steps.

Note that a rviz will also show up in order to visualize the map of the PAL office environment. 

{{attachment:multi_rviz.png||align="center", width="600"}}

Among other, the following information is overlaid on top of the map:

 * '''Global costmap''': regions around obstacles which are used by the global planner in order to compute paths to navigate from one point of the map to another without getting too close to the static obstacles registered during mapping. More details can be found here: http://wiki.ros.org/costmap_2d
 * '''Local costmap''': similar to the global costmap, but it is smaller and moves with the robot, it is used to take into account new features that were not present in the original map. It is used by the local planner to avoid obstacles, both static and dynamic, while trying to follow the global path computed by the global planner. More details can be found here: http://wiki.ros.org/costmap_2d
 * '''Laser scan''': lines in dark blue for the ''TIAGo1'' and magenta for the ''TIAGo2'' representing the points measured with the laser of the mobile base. This scan is used to add/remove obstacles both in the global and local costmaps.

Please refer to http://wiki.ros.org/Robots/TIAGo/Tutorials/Navigation/Localization to see the other available information.

== Autonomous navigation with rviz ==

First of all, go in the Tool Properties window of rviz and make sure that you have the 2D Pose Estimate and 2D Nav Goal topics selected on the specific namespace, as shown in the following picture.

{{attachment:multi_rviz_tool_prop_mark.png||align="center",width="600"}}

Due to we are working with multiple robots we have to add the tf_prefix of the robot to those topics, to make the two rviz tools working.

As can be seen in the pictures above, the robots wake up in the positions of the world that correspond to the ones we have set in the launch file (see http://wiki.ros.org/Robots/TIAGo/Tutorials/Multi_TIAGo_simulation for details). This is the pose that we provide also to the localization system. If the robot get lost or you need to change the initial pose you can use the ''2D Pose Estimate'' button in rviz to set the initial pose to seed the localization system (see http://wiki.ros.org/rviz/UserGuide for details).

In order to send the robot to another location of the map the ''2D Nav Goal'' button can be pressed in rviz.

{{attachment:multi_rviz_2dnav_mark.png||align="center",width="600"}}

Then, by clicking and holding the left button of the mouse over the point of the map at which we want to send the robot a green arrow will appear. By dragging the mouse around the point the orientation will change. This arrow represents the target position and orientation that the robot needs to reach. When releasing the mouse button the global planner will compute a path. The goal will appear as a red arrow representing the desired target orientation. Then, the robot will start moving following the trajectory, which will automatically re-arrange when getting too close to unexpected obstacles. 

The pictures below show different snapshots of rviz during the navigation toward the selected goal.

|| {{attachment:multi_rviz_goal1.png||width="350"}} || {{attachment:multi_rviz_goal2.png||width="350"}} ||

|| {{attachment:multi_rviz_goal3.png||width="350"}} || {{attachment:multi_rviz_goal4.png||width="350"}} ||

Finally, the robot stops when the target goal pose have been reached up to a user-defined tolerance.

Everything can be done also for ''TIAGo2''. Just deselect the ''TIAGo1'' displays in rviz, select the ''TIAGo2'' ones and then change the namespace for the topics in the Tool Properties window. Thus, you are able to execute the autonomous navigation also with the second TIAGo.

## AUTOGENERATED DO NOT DELETE
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE