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 : TP 4 — Le robot en 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 :
```
./scripts/ratbot up rooms ground_truth_tf:=false
```
Dans un deuxième terminal, la cartographie ; dans un troisième, le clavier :
```
./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 :

./scripts/ratbot map save 1 rooms_poste11

Questions

Pourquoi l’option ground_truth_tf:=false est-elle obligatoire ici ? (Voir le guide, La vérité terrain.)
Que se passe-t-il sur la carte si l’on tourne trop vite sur place ?
Quels fichiers map save a-t-il écrits ? Ouvrez l’image : que signifient le blanc, le noir et le gris ?

Étape 2 — Naviguer à la souris

Arrêtez la cartographie (Ctrl+C), gardez la simulation, et lancez la navigation sur votre carte :
```
./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.

Questions

Quelles sont les deux cartes de coûts affichées ? À quoi sert chacune ?
Que fait le robot face à l’obstacle ajouté ? Et si l’obstacle bouche complètement le passage ?
Donnez un but pendant que vous conduisez au clavier. Qui l’emporte ?

É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 :

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).

Questions

Que renvoie l’action pour un point placé dans un mur ?
Pendant un but, relancez patrouille.py dans un autre terminal. Que devient le premier but ?
Ce programme marcherait-il sur le vrai robot ? Que faudrait-il changer ?

Solution de l’étape 3

L’essentiel de patrouille.py :

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.

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.