Cartographie autonome d'un environnement clos

Introduction

On se propose de réaliser la cartographie d'un environnement clos de manière autonome. Tout le travail se fera avec un environnement et un robot simulé à l'aide de V-REP et le contrôle du robot se fera grâce à ROS. Pour ce faire, on dispose donc d'un environnement et d'un robot Pioneer 3dx. Le robot Pioneer 3dx qui vous est fourni est équipé de 16 capteurs ultrasons, d'une caméra et d'un laser scan de type Hokuyo installé en position frontale comme illustré ci-dessous.

La cartographie de l'environnement se fait en intégrant les informations fournies par le laserscan positionnée grâce à l'information d'odométrie du robot. Cela permet ainsi de construire une carte dans laquelle les cellules peuvent avoir un parmi trois états possibles: libre, obstacle, inconnu. L'état d'une cellule est déterminé grâce au laserscan dont la portée limitée permet de distinguer les cellules libres des obstacles. La problématique est alors que le robot navigue de manière autonome dans l'environnement afin qu'il n'y ait plus de cellule dans l'état inconnu atteignable par le robot. On se propose d'utiliser au maximum les outils fournis par ROS et notamment:

Avant de pouvoir utiliser ces outils, il nous faudra néanmoins commencer par définir le contrôleur qui permet de piloter le robot dans l'environnement. Votre travail comportera donc une partie d'intégration (gmapping, navigation stack) et une partie de développement (contrôleurs). Je vous conseille de développer vos noeuds avec l'interface python de ROS qui évite la phase de compilation du C++ et qui se prête bien à écrire les petits noeuds que nous avons à écrire.

Pour réaliser le TP, vous aurez besoin de plusieurs paquets qui ne sont pas installés par l'installation standard :

sudo apt install libopencv-contrib-dev libopencv-photo-dev libcv-dev
sudo apt install ros-kinetic-joy ros-kinetic-gmapping ros-kinetic-move-base ros-kinetic-map-server ros-kinetic-dwa-local-planner

Sujet

Prise en main de V-REP et de ROS

La simulation est réalisée à l'aide de V-REP. Commencez par récupérer l'archive de V-REP, version Pro-edu, sur le site de Coppelia Robotics. Téléchargez également le fichier de scène scene.ttt. Définissez dans votre fichier ~/.bashrc la variable VREP_ROOT qui pointe vers le répertoire où se trouve V-REP (A adapter en fonction du chemin d'installation de V-REP).

export VREP_ROOT=/home/fix_jer/Tools/V-REP_PRO_EDU_V3_4_0_Linux
Il vous faudra soit recharger votre fichier bashrc en exécutant source ~/.bashrc, soit vous fermez le terminal et en ouvrez un autre.

Pour que V-REP se connecte à ROS, il faut compiler quelques packages fournit par V-REP, qui se trouvent dans le répertoire de V-REP et sont à compiler avec catkin.

cp $VREP_ROOT/programming/ros_packages/v_repExtRosInterface ~/catkin_ws/src -r
cd ~/catkin_ws
catkin build
cp ~/catkin_ws/devel/lib/libv_repExtRosInterface.so  $VREP_ROOT/
	    

Une fois la configuration ROS/V-REP terminée, nous allons pouvoir lancer v-rep.

Assurez vous que dans la console, le plugin ROS soit correctement chargé par V-REP. Vous devez voir apparaitre la ligne :
login@hostname:~$ ./vrep.sh
...
Plugin 'RosInterface': loading...
Plugin 'RosInterface': load succeeded.
...
	    

Synchronisation des horloges de V-REP et de ROS

Dés que vous lancez roscore, une horloge interne à ROS se déclenche. Dés que vous lancez V-REP, une horloge interne à V-REP se lance. Nous avons deux horloges qu'il convient de mettre en accord. En fait, c'est assez simple, pour que ROS utilise une horloge externe, il suffit de publier sur le topic /clock des messages du type rosgraph_msgs/Clock et d'indiquer à ROS d'utiliser ce signal d'horloge en mettant le paramètre use_sim_time à vrai (e.g. rosparam set use_sim_time true). Vous avez deux possibilités pour définir le paramètre :

Il est conseillé de tout de suite commencer par définir un fichier launch, étant donné qu'à la fin du TP, un certain nombre de noeuds devront être instanciés.
Ayant précisé le paramètre use_sim_time, à chaque fois que vous redémarrerez la simulation depuis V-REP, ROS s'en rendra compte et pourra supprimer des éléments (e.g. des TFs) mis en tampon mais devenus invalides.

Contrôleur et odométrie

Le Pioneer p3dx est un robot à contrôle différentiel, i.e. il est équipé de deux roues séparées d'une distance $L=33cm$ et chacune de rayon $R=10cm$. Pour pouvoir contrôler le robot, on se donne une vitesse linéaire $v(t)$ et une vitesse angulaire $w(t)$ et on souhaite calculer les vitesses de rotation de la roue gauche $w_L$ et de la roue droite $w_R$. Pour un contrôle différentiel, la relation est donnée par : $$ \begin{eqnarray} R . w_L &= v - w . \frac{L}{2}\\ R . w_R &= v + w . \frac{L}{2} \end{eqnarray} $$

Vous devez écrire un noeud python pioneer_driver.py qui souscrit à un topic /cmd_vel et poste sur les topics de vitesse de roues /pioneer_p3dx/leftWheelCommand et /pioneer_p3dx/rightWheelCommand. Une fois votre contrôleur écrit, vous pouvez le tester :

Nous savons maintenant comment convertir un twist (vitesse linéaire et angulaire) en vitesse de rotation des deux roues. A titre anecdotique, les deux roues de notre robot sont équipées de roues codeuses qui indiquent la position angulaire des roues dans l'intervalle $[-\pi, \pi]$ sur les topics /pioneer_p3dx/leftWheelEncoder et /pioneer_p3dx/rightWheelEncoder. Le robot simulé dans V-REP fournit l'odométrie en postant sur le topic /pioneer_p3dx/odom et en postant la transformation entre le référentiel /Pioneer_p3dx/odom et le référentiel /Pioneer_p3dx sur le topic /tf. Prenez le temps d'observer ces différents référentiels dans rviz.

Localisation et mapping simultané (SLAM) avec gmapping

Nous disposons d'un topic d'odométrie et d'un topic de lasercan (sensor_msgs/LaserScan) de l'Hokuyo, que vous pouvez visualiser sous Rviz. D'ailleurs, on vous propose directement un fichier de configuration pour Rviz : rviz.config.base.rviz

Pour réaliser la cartographie, il nous faut combiner l'odométrie avec le laserscan. On va utiliser les noeuds du package gmapping pour réaliser la localisation et la cartographie simultanés de l'environnement (SLAM - Simultaneous Localization And Mapping). Gmapping va définir une carte (nav_msgs/OccupancyGrid), le référentiel /map, va intégrer régulièrement les informations du laserscan avec l'odométrie pour définir l'état des cellules de la carte (mapping) et va chercher à replacer en permanence le robot (localization) dans la carte en postant la transformation entre les référentiels /map et /Pioneer_p3dx/odom.

Utiliser gmapping se résume à instancier le noeud slam_gmapping correctement paramétré. Cela se ramène à définir un fichier launch qui devra contenir les choses suivantes :

Concernant la paramétrisation du noeud slam_gmapping dans le fichier de launch, voici quelques indications :

Vous êtes maintenant en mesure de cartographier l'environnement en contrôlant le robot manuellement (avec un teleop) et d'obtenir quelque chose comme sur la figure ci-dessous:

Planification avec la navigation stack

La prochaine étape consiste à mettre en place les noeuds qui permettent de planifier une trajectoire et de mettre en mouvement le robot. Nous allons pour cela utiliser la navigation stack et en particulier le noeud move_base du package move_base. La navigation stack va prendre en compte la position courante du robot, la carte actuelle de l'environnement, un but à atteindre ainsi que quelques éléments sur la géométrie et la dynamique du robot pour planifier une trajectoire et l'exécuter. Fondamentalement, move_base repose sur 3 notions :

Le planificateur local et le planificateur global utilisent chacun leurs propres costmaps. Ces différents éléments se définissent à l'aide de fichiers de paramètres de type yaml et chacun est extrêmement configurable. Il n'est pas nécessairement facile de correctement paramétrer move_base. Je vous propose une configuration qui devrait fonctionner relativement bien et qui se compose des 4 fichiers de configuration suivant :

J'ai ici choisi d'utiliser DWA plutôt que TrajectoryPlannerROS parce que DWA autorise des vitesses linéaires négatives ce qui permet de se sortir de certaines situations compliquées et le planificateur local TrajectoryPlannerROS ne semble pas l'autoriser

Vous placerez ces fichiers de configuration dans votre package et vous définirez un fichier launch move_base.launch qui contient :

Vous trouverez ci-dessous un extrait du fichier de launch dans lequel il faudra adapter les chemins.
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <param name="controller_frequency" value="5.0"/>
    <param name="recovery_behavior_enabled" value="false"/>
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
    <rosparam file="$(find robotique_autonome_be)/share/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find robotique_autonome_be)/share/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find robotique_autonome_be)/share/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find robotique_autonome_be)/share/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find robotique_autonome_be)/share/dwa_planner_ros.yaml" command="load" />
  </node>	    

En parcourant ces fichiers, vous verrez différents paramètres :

Une fois la navigation stack mise en place, vous pouvez spécifier des buts à votre robot depuis Rviz, en utilisant le bouton "2D Nav Goal". Lorsqu'un but est défini, vous pouvez visualiser sous Rviz les plans générés par les deux planificateurs. Vous pouvez également visualiser les topics de map publiés par les différents noeuds "Rviz/Add/By Topic" et voir les différentes costmap par exemple.

Exploration autonome

Il nous reste maintenant à écrire un contrôleur qui définisse des buts à notre robot, ces buts étant ensuite soumis au planificateur de la navigation stack pour planification et exécution. On vous fournit le squelette de noeud ROS/python pioneer_explorer_base.py qui contient quelques outils pour mettre en oeuvre le contrôleur, à savoir :

La classe Mapper vous permet par exemple d'exporter l'image ci-dessous.
Il ne vous reste plus qu'à écrire next_goal. Ci-dessous, une vidéo (un peu accélérée) d'un exemple qui ne marche pas trop mal, même si le planificateur a parfois du mal.











Ne descendez surtout pas plus bas
















Voici une solution possible du problème : robotique_autonome_be.tar.gz.

Jérémy Fix