Navigation autonome avec Nav2

À la page précédente, le robot savait comment il avait bougé (l’odométrie) mais pas où il se trouvait : il lui manquait une frame map, la référence absolue. Lui donner ce qui lui manque pour se déplacer vers un but dans un environnement qu’il découvre se fait en deux temps, qui structurent cette page :

1. Cartographier l’environnement pendant qu’on s’y localise — le SLAM (slam_toolbox) — puis sauvegarder la carte.
2. Naviguer dans cette carte : planifier et suivre des trajectoires en évitant les obstacles — la stack Nav2.

Le premier temps comble exactement le manque repéré à la page précédente : c’est le SLAM qui (re)construit la frame map et y ancre le robot.

1 · Cartographier l’environnement

SLAM = Simultaneous Localization And Mapping. Le robot construit une carte de son environnement en même temps qu’il s’y localise. Sur ROS 2, l’implémentation de référence pour des bases 2D avec LiDAR est slam_toolbox.

Entrées / sorties

Ce que slam_toolbox consomme et ce qu’il produit.

À vous de jouer

Une fois le SLAM lancé (section suivante), ouvrez rqt_graph dans un nouveau terminal pour retrouver ce schéma sur le graphe réel : /scan et /tf entrent dans slam_toolbox, /map en sort.

ros2 run rqt_graph rqt_graph

Ce qu’il faut surtout retenir : slam_toolbox publie la transformation map → odom, qui corrige la dérive de l’odométrie en recalant les scans laser. On obtient enfin la chaîne complète map → odom → base_footprint.

Lancer le SLAM (tout-en-un)

Un seul launch démarre Gazebo + SLAM + RViz (le scan apparaît en bleu) :

ros2 launch lekiwi_navigation slam.launch.py sim:=true

Variantes utiles

Le launch expose des options pour alléger ou adapter l’affichage :

# Gazebo sans GUI (un peu plus léger)
ros2 launch lekiwi_navigation slam.launch.py sim:=true headless:=true

# Couper RViz
ros2 launch lekiwi_navigation slam.launch.py sim:=true rviz:=false

Dans RViz, vous devez voir :

• l’OccupancyGrid /map apparaître au fur et à mesure ;
• la silhouette du LeKiwi suivre la base ;
• les rayons laser en couleur.

Téléop pour cartographier

Lancez en plus la téléop clavier (cf. première page) et baladez le robot dans le monde :

ros2 run lekiwi_bringup teleop_azerty

Quelques principes pour une bonne carte :

• Vitesses modérées (< 0,3 m/s) : les itérations SLAM ont le temps de fitter chaque scan.
• Boucle fermée : revenez régulièrement à un endroit déjà visité, ça aide l’algo à corriger la dérive (fermeture de boucle).
• Couverture complète : longez les murs pour qu’ils soient nets.

Sauvegarder la carte

Une fois satisfait, sauvegardez la carte dans un sous-dossier de maps/ (une carte = un dossier map.yaml + map.pgm). map_saver_cli ne crée pas le dossier : créez-le d’abord, puis sauvegardez :

# 1. créer le dossier de la carte
mkdir -p ~/ros2_bootcamp_ws/src/lekiwi_ros2/lekiwi_navigation/maps/bootcamp

# 2. sauvegarder la carte dedans
ros2 run nav2_map_server map_saver_cli \
  -f ~/ros2_bootcamp_ws/src/lekiwi_ros2/lekiwi_navigation/maps/bootcamp/map

Cela produit deux fichiers dans maps/bootcamp/ :

• map.yaml : métadonnées (origine, résolution, seuils) ;
• map.pgm : image grayscale de l’OccupancyGrid.

Vous pouvez les inspecter directement, hors ROS :

# métadonnées de la carte (origine, résolution, seuils)
cat ~/ros2_bootcamp_ws/src/lekiwi_ros2/lekiwi_navigation/maps/bootcamp/map.yaml

# image de l'OccupancyGrid, dans une visionneuse d'images
xdg-open ~/ros2_bootcamp_ws/src/lekiwi_ros2/lekiwi_navigation/maps/bootcamp/map.pgm
# ... ou : eog map.pgm

Lire le .pgm

L’image encode l’occupation par niveau de gris : blanc = libre, noir = mur / obstacle, gris = inconnu (jamais vu par le LiDAR). Une bonne carte a des murs nets et fins, peu de zones grises à l’intérieur de la pièce.

2 · Naviguer dans la carte

Nav2 est la stack de navigation de référence pour ROS 2 : planning global, suivi local, recovery, comportements, gestion des obstacles dynamiques.

Vue d’ensemble

Du goal jusqu’à /cmd_vel : planification globale, suivi local et costmaps.

Les briques importantes :

• bt_navigator : orchestre l’ensemble via un behavior tree (planifier, suivre, recovery si échec…).
• Planner : trajectoire globale point-à-point (Smac hybrid, NavFn…).
• Controller : suit le chemin localement en évitant les obstacles dynamiques (MPPI ici).
• Costmaps : représentations grille du monde (statique + locale) avec inflation autour des obstacles, exprimées dans base_footprint.

La pile complète tourne en 6 nœuds (controller, planner, behavior, bt_navigator, velocity_smoother, collision_monitor) gérés par un lifecycle_manager.

Contrôleur pour base holonome

Une base omnidirectionnelle (kiwi) peut translater sans tourner. Tous les controllers Nav2 ne le savent pas :

Controller	Holonome ?
DWB	Oui, à activer via vy_samples > 0 dans la config
MPPI	Oui, nativement (motion_model: Omni)
Regulated Pure Pursuit	Non, suppose une base différentielle

Sur LeKiwi, on utilise MPPI en motion_model: Omni : meilleur tracking et nativement holonome. Sa sortie est publiée sur /cmd_vel_nav (puis arbitrée par twist_mux).

Lancement

Le launch tout-en-un empile la sim, la localisation sur la carte bootcamp et la pile Nav2 complète, plus RViz, en une seule commande :

ros2 launch lekiwi_navigation navigation.launch.py slam_mode:=amcl map_name:=bootcamp

Premier goal au clic dans RViz

Dans RViz :

1. 2D Pose Estimate (bouton dans la toolbar) : cliquez la pose réelle du robot et orientez la flèche selon le cap. C’est la pose initiale du filtre de localisation.
2. 2D Goal Pose : cliquez la destination. Nav2 planifie et exécute la trajectoire.

Vous pouvez suivre l’avancement via :

ros2 topic echo /behavior_server/behavior_tree_log

Envoyer un goal par le code

Plutôt que de cliquer dans RViz, on peut envoyer un goal depuis Python via l’action navigate_to_pose. Téléchargez le client de départ et recopiez-le dans un package :

• 📄 go_to.py — client Nav2 minimal (un seul goal, avec suivi de la distance restante).

Ce client est générique : déposez-le dans le package ROS 2 de votre choix (<votre_package>) et déclarez son point d’entrée console_scripts dans le setup.py :

entry_points={
    "console_scripts": [
        "go_to = <votre_package>.go_to:main",
    ],
},

Puis, la navigation lancée, exécutez-le :

ros2 run <votre_package> go_to

Adaptez les coordonnées du goal (GOAL_X, GOAL_Y en tête de fichier) à votre carte — le monde bootcamp est une enceinte 8 × 8 m (origine au centre). Visez par exemple une zone repère, p. ex. zone_a en (-3, 3) (cf. exercice final).

Recoveries

Quand le robot est bloqué, Nav2 déclenche un comportement de recovery défini dans le behavior tree : spin, back_up, wait, puis clear_costmap ou réessai. Les bases holonomes peuvent utiliser drive_on_heading plutôt que back_up pour des manœuvres latérales.

3 · Exercice final — le tour des 3 zones

Objectif (capstone) : sur votre carte bootcamp (cartographiée et sauvegardée à la section 1), faites visiter au robot les trois zones repères du monde, dans l’ordre de votre choix, sans collision.

Le monde bootcamp est une pièce de 8 × 8 m (origine au centre, x et y de −4 à +4) contenant trois zones colorées :

Zone	Couleur	Position (x, y)	Emplacement
zone_a	🔴 rouge	(-3, 3)	coin haut-gauche
zone_b	🟢 vert	(3, 3)	coin haut-droite
zone_c	🔵 bleu	(0, -3)	bas, au centre

Lancez la navigation en localisation sur votre carte :

ros2 launch lekiwi_navigation navigation.launch.py slam_mode:=amcl map_name:=bootcamp

Tout se pilote par le code, sans aucun clic dans RViz — c’est la contrainte de l’exercice :

1. Initialisez la pose en publiant une fois sur /initialpose (geometry_msgs/PoseWithCovarianceStamped) au lieu du 2D Pose Estimate.
2. Choisissez votre ordre de visite (ex. zone_a → zone_c → zone_b) et enchaînez les trois poses automatiquement —
   • soit en adaptant go_to.py pour boucler sur une liste de goals ;
   • soit via l’action /follow_waypoints (waypoint_follower) : publiez les trois PoseStamped (l’orientation yaw finale est libre) et le robot les visite dans l’ordre, avec une pause à chaque zone.

Bonus : émettez un message ROS personnalisé à chaque zone atteinte (préparation aux Jours 5-6 — intégration).

Aller plus loin

Une fois la mission réussie, voici quelques pistes optionnelles pour approfondir.

Tuner le controller MPPI

Objectif : observer l’effet de quelques paramètres clés du controller sur le comportement du robot. Le fichier lekiwi_navigation/config/nav2/nav2.yaml contient toutes les options.

Jouez avec, un par un :

• vx_max, vy_max, wz_max (vitesses max — vy_max non nul = holonome) ;
• time_steps × model_dt (horizon de prédiction) ;
• critic_names : ajoutez / retirez ObstaclesCritic, GoalAngleCritic ;
• inflation_radius du global costmap (marge gardée autour des murs).

Pour chaque changement, relancez le même goal et notez : le temps total, la distance parcourue, le « confort » de la trajectoire (saccadée vs lisse).

Obstacle dynamique

Objectif : ajouter un cube dans Gazebo pendant que le robot navigue, et vérifier que Nav2 le contourne.

Indices :

• Ajoutez manuellement un cube dans Gazebo Ionic (gz-sim 9) pendant la sim (ou via un service create du monde).
• Le local_costmap doit s’allumer autour de l’objet quand le LiDAR le voit.
• Le controller MPPI doit replanifier localement.

Que se passe-t-il si vous placez l’objet exactement dans la trajectoire ? Combien de temps avant qu’il réagisse ?

Monitorer la navigation avec Foxglove

Objectif : inspecter en direct ce que produit la pile de navigation avec Foxglove, en complément de RViz.

Note

RViz reste l’outil de commande : le goal Nav2 se pose toujours dans RViz (2D Pose Estimate + 2D Goal Pose). Foxglove sert ici uniquement à observer.

Étapes :

1. Lancer la navigation (navigation.launch.py) puis le pont Foxglove dans un autre terminal :

   ros2 launch foxglove_bridge foxglove_bridge_launch.xml   # ws://localhost:8765

2. Se connecter à ws://localhost:8765 dans Foxglove et préparer un panneau 3D. Dans ses réglages, fixez Fixed frame = odom (ou map), puis activez les topics dans la section Topics :

   • /scan (sensor_msgs/LaserScan) — ce que « voit » le robot : les rayons du LiDAR se dessinent autour de lui et épousent les murs/obstacles. Ajustez taille et couleur des points dans les réglages du topic ;
   • /map (nav_msgs/OccupancyGrid) — la carte du SLAM, qui se remplit au fil du déplacement (n’apparaît que SLAM lancé) ;
   • les costmaps global et local pour voir l’inflation autour des obstacles.

3. Ajoutez en plus :

   • un panneau Plot sur les composantes de /cmd_vel ;
   • un panneau Raw Messages sur /odom pour relever une valeur de pose.

4. Poser un 2D Goal Pose dans RViz et observer, côté Foxglove : le /scan qui balaie, la trajectoire suivie, le local costmap qui s’allume autour des obstacles, et la courbe /cmd_vel.

Prochaine étape

Retour au sommaire J2, ou enchaînez sur Jour 3 — Manipulation.