[TOC]
# Exemple Navigation Automatique
#### Exemple de Robot :
### install ROS
```shell
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list
```
```shell
sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
```
```shell
sudo apt-get update
```
```shell
sudo apt-get install ros-kinetic-desktop-full
```
```shell
sudo rosdep init
rosdep update
```
```shell
source /opt/ros/kinetic/setup.bash
```
```shell
echo "source /opt/ros/kinetic/setup.zsh" >> ~/.zshrc
source ~/.zshrc
```
Dependencies for building packages
```shell
sudo apt install python-rosinstall python-rosinstall-generator python-wstool build-essential
```
### install catkin worspace
Création du repertoire de travail.
```shell=
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/
catkin_make
echo source $PWD/devel/setup.bash >> ~/.bashrc
source ~/.bashrc
```
### Exemple de robot/vehicule :
* Pull le robot dans le dossier de travail **~/ws_catkin/src**.
* ``` git clone https://github.com/vibhuthasak/Obstacle_Avoidance_ROS.git```
* ```cd .. && catkin_make```
* ```source ~/.bashrc```
#### Lancer le robot
Pour lancer le robot, lancez la cammnde :
```ros
roslaunch testbot_description testbot_gazebo.launch
```
Résultat :
Pour regler l'erreur suivant, affiché dans le terminal :
il faudra rajouter la ligne 176 dans le fichier suivant:
**/home/test/catkin_ws/src/Obstacle_Avoidance_ROS/urdf/my_robot_tutorial.urdf**
``` xml=156
<gazebo>
<plugin filename="libgazebo_ros_diff_drive.so" name="differential_drive_controller_front">
<rosDebugLevel>Debug</rosDebugLevel>
<publishWheelTF>false</publishWheelTF>
<robotNamespace>/</robotNamespace>
<publishTf>1</publishTf>
<publishWheelJointState>false</publishWheelJointState>
<alwaysOn>true</alwaysOn>
<updateRate>100.0</updateRate>
<leftJoint>left_wheel_joint</leftJoint>
<rightJoint>right_wheel_joint</rightJoint>
<wheelSeparation>1.5</wheelSeparation>
<wheelDiameter>0.4</wheelDiameter>
<broadcastTF>1</broadcastTF>
<wheelTorque>30</wheelTorque>
<wheelAcceleration>1.8</wheelAcceleration>
<commandTopic>cmd_vel</commandTopic>
<odometryFrame>odom</odometryFrame>
<odometryTopic>odom</odometryTopic>
<robotBaseFrame>base_footprint</robotBaseFrame>
<legacyMode>true</legacyMode>
</plugin>
</gazebo>
```
-----------------------------
Et maintenant on observe les messages publiées par le robot :
```shell=
rostopic list
```
Comme on peut le voir sur le terminal le robot publie pas de messages GPS ni IMU qui servent à la navigation autonome, donc il faudra les rajouter.
## les Capteurs :
Pour que le robot puisse naviguer en autonome il a besoin de plusieurs capteurs qu'on doit lui rajouter, parmis lesquels :
### IMU : inertial measurement unit, « centrale à inertie » :
Pour équiper notre robot d'un capteur pareil :
* Dans le fichier : **/catkin_ws/src/Obstacle_Avoidance_ROS/urdf/my_robot_tutorial.urdf**
* Ajouter ces lignes à l'interieur de l'element **robot**:
```xml=
<gazebo>
<plugin name="imu_plugin" filename="libgazebo_ros_imu.so">
<alwaysOn>true</alwaysOn>
<bodyName>base_link</bodyName>
<topicName>imu</topicName>
<serviceName>imu_service</serviceName>
<gaussianNoise>0.0</gaussianNoise>
<updateRate>20.0</updateRate>
</plugin>
</gazebo>
```
### GPS : Global Positioning System, « système de géolocalisation par satellite » :
Pour équiper notre robot d'un **GPS**,
dans le même fichier il faudra rajouter :
```xml=
<gazebo>
<plugin name="gazebo_ros_gps" filename="libhector_gazebo_ros_gps.so">
<updateRate>40</updateRate>
<bodyName>base_link</bodyName>
<frameId>base_link</frameId>
<topicName>/gps/fix</topicName>
<velocityTopicName>/gps/fix_velocity</velocityTopicName>
<referenceLatitude>29.45196669</referenceLatitude>
<referenceLongitude>-98.61370577</referenceLongitude>
<referenceHeading>90</referenceHeading>
<referenceAltitude>0</referenceAltitude>
<offset>0 0 0</offset>
<drift>0.001 0.001 0.001</drift>
<gaussianNoise>0.05 0.05 0.05</gaussianNoise>
<velocityDrift>0.001 0.001 0.001</velocityDrift>
<velocityGaussianNoise>0.5 0.5 0.5</velocityGaussianNoise>
<drift>0.001 0.001 0.001</drift>
<gaussianNoise>0.01 0.01 0.01</gaussianNoise>
<velocityDrift>0 0 0</velocityDrift>
</plugin>
</gazebo>
```
le plugin GPS est fourni par
[hector_gazebo_plugins](http://wiki.ros.org/hector_gazebo_plugins)
Donc il faut installer le pquet :
```shell=
sudo apt-get install ros-$ROS_DISTRO-hector-gazebo-plugins
```
## lancer la simulation dans gazebo :
Aretez le processus dans le terminal et relancer encore une fois la simulation :
``` ros
roslaunch Obstacle_Avoidance_ROS testbot_gazebo.launch
```
Et maintenant vous devriez avoir des messages GPS et IMU publiées par ces capteurs :
```shell=
rostopic list
```
### Pile de navigation
Pour implementer la pile de navigation sur notre robot on va se servir des exemples dèjas existants, on va prendre donc l'exemple d'un tutoriel officiel.
[TUTORIAL](http://wiki.ros.org/teb_local_planner/Tutorials/Track%20and%20include%20dynamic%20obstacles%20via%20costmap_converter)
```shell=
cd ~/catkin_ws/src/
git clone https://github.com/rst-tu-dortmund/teb_local_planner_tutorials
cd ..
catkin_make
source ~/.bashrc
```
Pour pouvoir lancer le tutoriel on doit installer quelques packages necessaires pour le fonctionnement de [local_planner](http://wiki.ros.org/teb_local_planner) ainsi qu'au pile de [navigation](http://wiki.ros.org/navigation)...
liste des packages à installer :
```shell=
sudo apt-get install ros-kinetic-teb-local-planner
sudo apt-get install ros-kinetic-map-server
sudo apt-get install ros-kinetic-move-base*
sudo apt-get install ros-kinetic-costmap-converter
sudo apt-get install ros-kinetic-gmapping
source ~/.bashrc
#si les paquets n'on pas été tus installé sur Melodic
cd ~/catkin_ws/src
git clone https://github.com/ros-planning/navigation.git
cd ..
rosdep install --from-paths src --ignore-src #dépendances
catkin_make
source ~/.bashrc
```
Pour lancer l'exemple qui va nous servir comme environnement dàjas configurée pour la pile de navigation, il faut lancer :
```ros
roslaunch teb_local_planner_tutorials dyn_obst_costmap_conversion.launch
```
Et vous devez avoir des fenetres ouvertes comme sur l'image :
la fenêtre en bas agauche represente la simulation, et la fenetre adroite permet de viziualiser l'ensemble des données venants des capteurs ainsi que des données generées par des algorithmes et packages tels que les scans, cartes, obstacles, chemins...
L'exemple devant nous represente un robot(**cercle**) et un obstacle dynamique qui se retrouvent dans une carte fermée sa veut dire que le robot ne peut pas depasser cet carte dabord car il considere l'entourage comme obstacle et deuxiement la navigation ne comprend pas l'exterieur de la carte.
la fenetre adroite permet aussi de publier des messages/commandes, comme par exemple la flesh verte tout en haut **2D Nav Gaol**, permet d’envoyer un but à la navigation en définissant la position souhaitée, au robot pour l'atteindre, il suffit juste de cliquer dessus et après choisir un point à lterieur de la carte representée en carré.
Et maintenant on va remplacer la scene de simulation du tutoriel avec notre scene de gazebo qui comporte notre robot.
pour le faire on va modifier le ficher
```shell=
gedit /home/test/catkin_ws/src/teb_local_planner_tutorials/launch/dyn_obst_costmap_conversion.launch &
```
:::info
Le fichier après toutes les modifications sera donné à la fin.
:::
Les étapes à faire dans chaque partie :
#### la partie : ************** Stage Simulator *****************
* Enlever la prtie qui permet de lancer le [stage](http://wiki.ros.org/stage) pour lancer notre scene gazebo, remplacez cette partie par :
```xml=
<!-- ************** Gazebo Simulator ***************** -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find testbot_description)/worlds/assignment_world.world"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="headless" value="$(arg headless)"/>
<arg name="debug" value="$(arg debug)"/>
</include>
<param name="robot_description" textfile="$(find testbot_description)/urdf/my_robot_tutorial.urdf" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" >
<param name="publish_frequency" type="double" value="50.0" />
</node>
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
args="-z 1.0 -unpause -urdf -model robot -param robot_description" respawn="false" output="screen" />
```
### ******************* Maps ***********************
Cette partie pemret de lancer le [map_server](http://wiki.ros.org/map_server) en precisant le fichier **.yaml** de configuration, où y'a marqué le nom de l'image qui sera servi comme carte ainsi que les paramêtres de dimensions...
le fichier **yaml** :
```shell=
gedit catkin_ws/src/teb_local_planner_tutorials/maps/empty_box.yaml &
```
On peut changer la carte, qui se trouve dans le même dossier .../maps on peut mettre par exemple une image blanche pour dire que la carte est vide sans obstacles...
### ******** Localization *********
Dans cette partei on va supprimer le deuxieme **node** car il concerne l'obstacle que dans gazebo on va pas pouvoir l'afficher.
et après remplacer le topic ==robot_0/odom== par **odom** celui de notre robot.
Et on met les valeurs de args à 0 :
```xml=
<node pkg="tf" type="static_transform_publisher" name="perfect_loc_robot" args="0 0 0 0 0 0 /map /odom 100" />
```
### ************* Navigation Ego Robot ************
Cette partie permet de lancer la pile de navigation avec tous les parametres necessaires, en ce moment on peut tout laisser comme il est, mais on va modifier les fichiers auquels on fait appel :
dans les fichiers
* /catkin_ws/src/teb_local_planner_tutorials/cfg/diff_drive/dyn_obst/local_costmap_params.yaml
* /catkin_ws/src/teb_local_planner_tutorials/cfg/diff_drive/dyn_obst/global_costmap_params.yaml
il faut remplacer **robot_0/base_link** par **/base_link**
Dans le fichier :
* /catkin_ws/src/teb_local_planner_tutorials/cfg/diff_drive/dyn_obst/teb_local_planner_params.yaml
il faut changer :
```yaml=
footprint_model:
type: "point"
```
en
```yaml=
footprint_model:
radius: 0.9
```
Dans le fichier
/home/test/catkin_ws/src/teb_local_planner_tutorials/cfg/diff_drive/costmap_common_params.yaml
il faut changer
```yaml=
robot_radius: 0.17
```
en
```yaml=
robot_radius: 0.9
```
Mettre **raduis** à la place de **point** permet de dire que le robot est de forme **cercle** de tel rayon.
### ******************* Obstacles *******************
Dans cette partie on peut supprimer **```<group ns="robot_1">```** qui concerne l'obstacle, et changer :
* **/robot_0/move_base/local_costmap/costmap** en ==/move_base/local_costmap/costmap==
* **/robot_0/odom** en ==/odom==
### **************** Visualisation ****************
Pereil que précédemment, changez :
* **/robot_0/move_base_simple/goal** en ==/move_base_simple/goal==.
Cette partie permet de lancer **rviz** avec une configuration dèjas prête qu'on doit changer car les noms topics vont changer puisque on a remplacer le robot.
chemin : **teb_local_planner_tutorials/cfg/rviz_navigation_cc.rviz**
Telecharger le [Nouveau](https://github.com/tajaouart/rviz_configuration/blob/master/rviz_navigation_cc.rviz) fichier de configuration.
-----------------------------------------------------------------------
Après avoir faire toutes les étapes, le fichier **dyn_obst_costmap_conversion.launch** doit ressembler à :
```xml=
<launch>
<arg name="map_file" default="empty_box"/>
<!-- ************** Global Parameters *************** -->
<param name="/use_sim_time" value="true"/>
<!-- ************** Gazebo Simulator ***************** -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find testbot_description)/worlds/assignment_world.world"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="headless" value="$(arg headless)"/>
<arg name="debug" value="$(arg debug)"/>
</include>
<param name="robot_description" textfile="$(find testbot_description)/urdf/my_robot_tutorial.urdf" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" ></node>
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen" >
<param name="publish_frequency" type="double" value="50.0" />
</node>
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
args="-z 1.0 -unpause -urdf -model robot -param robot_description" respawn="false" output="screen" />
<!-- ******************* Maps *********************** -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find teb_local_planner_tutorials)/maps/$(arg map_file).yaml" output="screen">
<param name="frame_id" value="map"/>
</node>
<!-- ******** Localization ********* -->
<!-- See stage world file for initial poses -->
<node pkg="tf" type="static_transform_publisher" name="perfect_loc_robot" args="-2 0 0 -1.570796 0 0 /map /odom 100" />
<!-- ************* Navigation Ego Robot ************ -->
<group>
<!-- work around for footprint reload -->
<rosparam command="delete" ns="move_base" />
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find teb_local_planner_tutorials)/cfg/diff_drive/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find teb_local_planner_tutorials)/cfg/diff_drive/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find teb_local_planner_tutorials)/cfg/diff_drive/dyn_obst/local_costmap_params.yaml" command="load" />
<rosparam file="$(find teb_local_planner_tutorials)/cfg/diff_drive/dyn_obst/global_costmap_params.yaml" command="load" />
<rosparam file="$(find teb_local_planner_tutorials)/cfg/diff_drive/dyn_obst/teb_local_planner_params.yaml" command="load" />
<!-- Here we load our costmap conversion settings -->
<!--rosparam file="$(find teb_local_planner_tutorials)/cfg/diff_drive/dyn_obst/costmap_converter_params.yaml" command="load" /-->
<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
<param name="controller_frequency" value="5.0" />
<param name="controller_patience" value="15.0" />
<remap from="map" to="/map"/>
</node>
</group>
<!-- ******************* Obstacles ******************* -->
<node name="standalone_converter" pkg="costmap_converter" type="standalone_converter" output="screen">
<param name="converter_plugin" value="costmap_converter::CostmapToDynamicObstacles" />
<param name="costmap_topic" value="/move_base/local_costmap/costmap" />
<param name="odom_topic" value="/odom" />
</node>
<!-- **************** Visualisation **************** -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find teb_local_planner_tutorials)/cfg/rviz_navigation_cc.rviz">
<remap from="/move_base_simple/goal" to="/move_base_simple/goal" />
</node>
</launch>
```
---------------------------------------------------------------
Et si vous essayer maintenant de lancer la commande
```shell=
roslaunch teb_local_planner_tutorials dyn_obst_costmap_conversion.launch
```
vous devrez avoir sur **rviz**:
### Parametres avancées :
#### le Fichier yaml de la carte :
/home/test/catkin_ws/src/teb_local_planner_tutorials/maps/empty_box.yaml
```yaml=
image: empty_box.png
resolution: 0.050000
origin: [-3.0, -3.0, 0.0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
```
* **image** : le nom de l'image/carte.
* **reolution** : resolution de la map, metres/Pixels.
* **origine** : la position du robot dans l'image/carte ( (0,0,orientation) est le bas agauche)
* **occupied_thresh** : les pixels dont la probabilité d'occupation est supérieure à ce seuil sont considérés comme complètement occupés.
* **free_thresh** : les pixels dont la probabilité d'occupation est inférieure à ce seuil sont considérés comme totalement libres.
* **negate** : choisir si la sémantique blanc / noir libre / occupé doit être inversée (l'interprétation des seuils n'est pas affectée)
Donc pour agrandir notre carte il suffit d'augmenter la valeur de **resolution** pour dire que il y'a plus de metres dans un pixel.
```yaml=
...
resolution: 0.500000 #agrandir l'image
origin : [-30.0, -30.0, 0.0] # (120 * 0.5)/2 =30 120 : dimension en pixel
...
```
résultat après le relancement:
A cette étape on peu essayer la navigation du robot avec l'outil **2D Nav Goal** de **rviz** comme je l'ai exmpliqué tout an haut.
--------------------------------------------
### carte satellite :
#### mapviz :
**MapViz** est un outil de visualisation similaire à **rviz** qui se concentre sur la visualisation de données 2D sur une carte satellite.
##### installation :
pour l'installer :
```shell=
cd ~/catkin_ws/src
git clone https://github.com/swri-robotics/mapviz.git
cd ..
rosdep install --from-paths src --ignore-src
catkin_make
source ~/.bashrc
```
pour lancer mapviz :
```shell=
roslaunch mapviz mapviz.launch
```
Pour afficher les donnée du robot, il faut selectionner **base_link** pour ==Target Frame==.
Pour afficher la position **GPS**, il suffit de cliquer sur le bouton **add** en bas et après choisir **navSat**, puis selectionner le nom du topic qui est dans notre cas **/gps/fix**.
de la même maniere on peut ajouter la carte satellite, en selectionnant le plugin ==tile_map==.
----------------------------
#### Quelques plug-ins importants :
* **navsat** : GPS.
* **tile_map** : carte sattelite.
* **tf_frame** : position du robot.
* **move_base** : publier des messages/commandes de navigation.
* **occupanct_grid** : affiche la carte gmapping.
* **path** : affiche le chemin vers la destination.
* **odmetry** : direction.
Pour pouvoir lancer mapviz avec l'ensemble du projet on peu mettre ce qui suit dans le fichier /home/test/catkin_ws/src/teb_local_planner_tutorials/launch/dyn_obst_costmap_conversion.launch
```xml=
<!-- ==================== MapViz =========================-->
<node pkg="mapviz" type="mapviz" name="mapviz"></node>
<node pkg="swri_transform_util" type="initialize_origin.py" name="initialize_origin" >
<param name="local_xy_frame" value="/map"/>
<param name="local_xy_origin" value="swri"/>
<rosparam param="local_xy_origins">
[{ name: swri,
latitude: 29.45196669,
longitude: -98.61370577,
altitude: 233.719,
heading: 0.0},
{ name: back_40,
latitude: 29.447507,
longitude: -98.629367,
altitude: 200.0,
heading: 0.0}]
</rosparam>
</node>
```
### Navigation GPS :
étapes pour tester la navigation GPS avec le fichier en bas:
* se connecte à move_base.
* récupere les coordonnées du robot.
* récupere les gps goals.
* transforme les coordonnées gps en coordonneés locals(x,y...)
* publie un message à move_base pour le point 1 et puis 2, ...
```shell=
cd ~/catkin_ws/src
git clone https://github.com/tajaouart/gps_goals.git
cd gps_goals
chmod 777 nav_gps.py
sudo apt install python-pip
pip install geographiclib
./nav_gps.py
```
sur le terminal vous devriez suivre les instructions pour saisir les gps_goas.
:::warning
Pour le fonctionnement de la navigatios il faut que les GPS-Goals soit à l'interieur de la carte affiché sur **MapViz** grâce à **occupanct_grid**
:::
Lien externes :
https://answers.ros.org/question/258307/gazebo-simulation-gps-fix/
Sign in with Wallet
Connect another wallet