TP 5 — Cartographier et naviguer ================================ Objectifs --------- - construire la **carte** d'un lieu avec le lidar (SLAM) ; - y **localiser** le robot, puis lui envoyer des **buts** ; - piloter la navigation depuis un programme, par une **action** ROS 2. Durée indicative : deux séances de 3 h. Prérequis : :doc:`tp04_simulation`. Ce qu'il faut savoir -------------------- La carte et la position ~~~~~~~~~~~~~~~~~~~~~~~ L'odométrie (TP 4) dérive : après quelques mètres, la position estimée s'écarte de la vraie. Le **lidar**, lui, voit les murs. En comparant ce qu'il voit à une **carte**, on corrige la dérive. - **Cartographier** (*SLAM*, *Simultaneous Localization And Mapping*) : le robot construit la carte **en même temps** qu'il s'y localise. - **Naviguer** : la carte est connue ; **AMCL** y localise le robot, et **Nav2** calcule un chemin vers le but et le suit en évitant les obstacles. Ces deux étapes sont décrites dans le guide : `Faire une carte `_ et `Navigation autonome `_. Lisez-les avant le TP. Les actions ~~~~~~~~~~~ Atteindre un but prend du temps : plusieurs secondes, parfois un échec. Un service, qui attend une réponse unique, ne convient pas. ROS 2 propose les **actions** : on envoie un **but**, on reçoit des **retours** pendant l'exécution (la distance restante), puis un **résultat** ; et on peut **annuler** en cours de route. Nav2 offre l'action ``navigate_to_pose``, de type ``nav2_msgs/action/NavigateToPose``. Étape 1 — Faire la carte ------------------------ #. Lancez la simulation dans le décor ``rooms``, **sans vérité terrain** : .. code-block:: bash ./scripts/ratbot up rooms ground_truth_tf:=false #. Dans un deuxième terminal, la cartographie ; dans un troisième, le clavier : .. code-block:: bash ./scripts/ratbot slam 1 ./scripts/ratbot key 1 #. Parcourez **tout** le décor, lentement, en repassant par le point de départ. Regardez la carte se construire dans RViz. #. Enregistrez-la : .. code-block:: bash ./scripts/ratbot map save 1 rooms_poste11 .. Capture d'écran à insérer : RViz pendant la cartographie du décor rooms, la carte en train de se dessiner. Déposez _static/captures/ros/tp05-slam.png puis décommentez le bloc ci-dessous (retirez le préfixe « .. » devant chaque ligne) : .. .. image:: ../_static/captures/ros/tp05-slam.png .. :align: center .. :width: 90% .. admonition:: Questions 1. Pourquoi l'option ``ground_truth_tf:=false`` est-elle obligatoire ici ? (Voir le guide, *La vérité terrain*.) 2. Que se passe-t-il sur la carte si l'on tourne trop vite sur place ? 3. Quels fichiers ``map save`` a-t-il écrits ? Ouvrez l'image : que signifient le blanc, le noir et le gris ? .. dropdown:: Réponses aux questions 1 à 3 :icon: light-bulb :color: info **1. ground_truth_tf:=false.** Un repère ne peut avoir **qu'un seul parent** dans l'arbre TF. Par défaut, la simulation relie déjà le repère ``odom`` du robot au monde (``world``), par la position exacte qu'elle connaît : la *vérité terrain*. La cartographie, elle, publie sa propre estimation, ``map → odom``. Deux sources pour situer ``odom`` : l'arbre est incohérent, et la cartographie (comme la localisation) échoue ou donne des résultats absurdes. Sur le vrai robot, la question ne se pose pas : aucune vérité terrain n'existe. **2. Tourner trop vite.** La carte se **dédouble** ou se **tord** : des murs en double, décalés d'un angle. Le SLAM recale chaque tour de lidar sur la carte déjà construite, en partant de l'estimation de l'odométrie. Une rotation rapide pendant le tour du lidar (100 ms) le déforme, et l'odométrie, moins précise en rotation qu'en ligne droite, donne un point de départ trop éloigné : le recalage tombe dans un mauvais minimum, et l'erreur s'inscrit dans la carte. D'où la consigne : **lentement**, et en repassant par des endroits déjà vus, ce qui permet au SLAM de **fermer les boucles** et de corriger la dérive accumulée. **3. Les fichiers.** ``map save`` écrit deux fichiers dans ``ros2_ws/src/ratbot_maps/maps`` : **rooms_poste11.pgm**, une image en niveaux de gris, et **rooms_poste11.yaml**, qui la décrit (résolution en mètres par pixel, position de l'origine, seuils). Dans l'image : **blanc** = libre, **noir** = occupé (un obstacle), **gris** = inconnu (jamais vu par le lidar). C'est une **grille d'occupation** : chaque case de 5 cm porte la probabilité qu'elle soit occupée, réduite à trois valeurs pour la sauvegarde. Étape 2 — Naviguer à la souris ------------------------------ #. Arrêtez la cartographie (Ctrl+C), gardez la simulation, et lancez la navigation sur **votre** carte : .. code-block:: bash ./scripts/ratbot nav 1 rooms_poste11 #. Dans RViz, si besoin, donnez la position du robot (**2D Pose Estimate**), puis un but (**2D Goal Pose**). #. Pendant que le robot roule, ajoutez un obstacle dans Gazebo (une boîte) sur son chemin. .. Capture d'écran à insérer : RViz en navigation : la carte, les deux cartes de coûts, le chemin planifié. Déposez _static/captures/ros/tp05-navigation.png puis décommentez le bloc ci-dessous (retirez le préfixe « .. » devant chaque ligne) : .. .. image:: ../_static/captures/ros/tp05-navigation.png .. :align: center .. :width: 90% .. admonition:: Questions 4. Quelles sont les deux cartes de coûts affichées ? À quoi sert chacune ? 5. Que fait le robot face à l'obstacle ajouté ? Et si l'obstacle bouche complètement le passage ? 6. Donnez un but pendant que vous conduisez au clavier. Qui l'emporte ? .. dropdown:: Réponses aux questions 4 à 6 :icon: light-bulb :color: info **4. Les deux cartes de coûts.** - La **carte de coûts globale** couvre toute la carte : elle sert au **planificateur global**, qui calcule le chemin complet jusqu'au but. Elle contient les murs de la carte enregistrée, **gonflés** de la marge de sécurité (25 cm) autour desquels le centre du robot ne doit pas passer. - La **carte de coûts locale**, quelques mètres autour du robot, se met à jour en permanence avec le lidar (et le capteur de distance avant) : elle sert au **contrôleur local**, qui suit le chemin en tenant compte des obstacles imprévus. Deux cartes, parce que deux problèmes : trouver un chemin sur une grande carte peut prendre du temps et se refait rarement ; éviter un obstacle doit se faire vite, plusieurs fois par seconde, mais seulement près du robot. **5. L'obstacle ajouté.** Le lidar le voit, il apparaît dans les cartes de coûts, et le robot le **contourne** : le contrôleur local s'écarte, et le planificateur global recalcule un chemin qui en tient compte. Si l'obstacle **bouche complètement** le passage, aucun chemin n'existe : Nav2 lance ses **comportements de récupération** (attendre, reculer, tourner sur place, effacer les cartes de coûts), puis réessaie, et finit par **abandonner** le but au bout d'un nombre d'essais. **6. Le clavier contre la navigation.** Le **clavier** l'emporte : la navigation publie sur ``cmd_vel_nav``, la priorité la plus basse de twist_mux (TP 4). Quand on lâche le clavier, la navigation reprend la main 0,5 s plus tard, et continue vers son but depuis la nouvelle position. On peut ainsi aider le robot à sortir d'une impasse sans annuler la navigation. Étape 3 — Une patrouille programmée ----------------------------------- Écrivez ``patrouille.py`` : le robot visite une liste de points, dans l'ordre, puis revient au départ. #. Créez un **client d'action** : .. code-block:: python from rclpy.action import ActionClient from nav2_msgs.action import NavigateToPose self.client = ActionClient(self, NavigateToPose, '/ratbot_1/navigate_to_pose') #. Pour chaque point : construisez un ``PoseStamped`` dans le repère ``map`` (orientation en quaternion : ``z = sin(θ/2)``, ``w = cos(θ/2)``), envoyez le but avec ``send_goal_async``, attendez qu'il soit **accepté**, puis attendez le **résultat**. #. Affichez la distance restante (``feedback.distance_remaining``) toutes les deux secondes. #. Si un but échoue, passez au suivant, et comptez les réussites. Choisissez des points **libres** sur votre carte (survolez-la dans RViz : les coordonnées s'affichent en bas de la fenêtre). .. admonition:: Questions 7. Que renvoie l'action pour un point placé **dans** un mur ? 8. Pendant un but, relancez ``patrouille.py`` dans un autre terminal. Que devient le premier but ? 9. Ce programme marcherait-il sur le vrai robot ? Que faudrait-il changer ? .. dropdown:: Solution de l'étape 3 :icon: code :color: secondary L'essentiel de ``patrouille.py`` : .. code-block:: python from rclpy.action import ActionClient from geometry_msgs.msg import PoseStamped from nav2_msgs.action import NavigateToPose from action_msgs.msg import GoalStatus POINTS = [(1.5, 0.0, 0.0), (2.0, 1.5, 90.0), (-1.5, 1.5, 180.0), (0.0, 0.0, 0.0)] def pose(node, x, y, yaw_deg): p = PoseStamped() p.header.frame_id = 'map' p.header.stamp = node.get_clock().now().to_msg() p.pose.position.x = x p.pose.position.y = y a = math.radians(yaw_deg) p.pose.orientation.z = math.sin(a / 2.0) # rotation autour de z seulement p.pose.orientation.w = math.cos(a / 2.0) return p class Patrouille(Node): def __init__(self, robot): super().__init__('ratbot_lab_patrouille') self.client = ActionClient(self, NavigateToPose, f'/ratbot_{robot}/navigate_to_pose') self.last_print = 0.0 def on_feedback(self, fb): now = self.get_clock().now().nanoseconds / 1e9 if now - self.last_print >= 2.0: self.last_print = now self.get_logger().info(f' reste {fb.feedback.distance_remaining:.2f} m') def go(self, x, y, yaw): goal = NavigateToPose.Goal() goal.pose = pose(self, x, y, yaw) send = self.client.send_goal_async(goal, feedback_callback=self.on_feedback) rclpy.spin_until_future_complete(self, send) # 1. but accepté ? handle = send.result() if not handle.accepted: return False res = handle.get_result_async() rclpy.spin_until_future_complete(self, res) # 2. résultat return res.result().status == GoalStatus.STATUS_SUCCEEDED et dans ``main`` : ``client.wait_for_server(timeout_sec=30.0)``, puis ``go`` pour chaque point, en comptant les réussites. Dans le décor ``rooms`` en simulation : **4 buts sur 4** atteints. **Les trois temps d'une action.** ``send_goal_async`` envoie le but, et renvoie un *future* : une promesse de réponse. Le serveur répond d'abord s'il **accepte** le but (il peut le refuser, s'il est mal formé par exemple) ; ensuite arrivent les **retours** (``feedback_callback``, ici la distance restante, plusieurs fois par seconde) ; enfin le **résultat**, avec un statut : réussi, abandonné, annulé. ``spin_until_future_complete`` fait tourner le nœud (donc reçoit les retours) jusqu'à ce que la promesse soit tenue. **Le quaternion d'un angle.** Pour une rotation d'angle θ autour de z, le quaternion vaut (0, 0, sin θ/2, cos θ/2) : la formule inverse de celle du TP 4. **Le repère map.** Les buts sont donnés dans le repère de la **carte** : les coordonnées qu'on lit en survolant la carte dans RViz. .. dropdown:: Réponses aux questions 7 à 9 :icon: light-bulb :color: info **7. Un point dans un mur.** Le but est **accepté** (il est bien formé), mais le planificateur ne trouve aucun chemin vers une case occupée : après ses tentatives, Nav2 **abandonne**, et le résultat a le statut ``STATUS_ABORTED``. La patrouille le compte comme un échec et passe au point suivant. Un programme robuste vérifie toujours le statut final, au lieu de supposer que le robot est arrivé. **8. Deux patrouilles en même temps.** Le serveur de navigation n'exécute qu'**un but à la fois** : le nouveau but **remplace** le premier, qui est interrompu (le premier client reçoit un statut d'abandon ou d'annulation, et passe au point suivant… qui remplacera à son tour celui de l'autre). Les deux programmes se disputent le robot. Un seul programme doit décider des buts d'un robot : c'est le rôle d'un **gestionnaire de mission**. **9. Sur le vrai robot.** Oui, **à l'identique** : mêmes topics, même action, même nom de robot. C'est tout l'intérêt de la simulation. Il faut simplement : - une **carte du vrai lieu** (``ratbot slam`` puis ``ratbot map save`` avec le vrai robot) ; - des **points libres** dans cette carte ; - le **domaine 77**, celui des robots ; - et un robot correctement localisé au départ (*2D Pose Estimate*). .. dropdown:: Tout le code de la correction :icon: download :color: secondary :download:`patrouille.py <../../corrections/ros/tp05/patrouille.py>` — essayé en simulation (décor ``rooms``, ``ratbot nav 1``). Il se lance directement, après ``source /opt/ros/humble/setup.bash`` et ``export ROS_DOMAIN_ID=…`` : ``python3 patrouille.py [N] [TOURS]``. Exercices supplémentaires ------------------------- Ces exercices n'ont pas de correction. - **Annuler** : un appui sur une touche (ou un message sur un topic) annule le but en cours (``cancel_goal_async``) et renvoie le robot au départ. - **Trajets appris** : essayez ``ratbot route record`` puis ``ratbot route follow`` (guide : *Trajets*). Comparez avec votre patrouille. - **L'exploration autonome** : ``ratbot explore 1`` cartographie seul un lieu inconnu (guide : *Exploration*). Comparez la carte obtenue à la vôtre. Après le TP ----------- Arrêtez la navigation, puis la simulation. Votre carte reste dans ``ros2_ws/src/ratbot_maps/maps``.