TP 4 — Le robot en simulation ============================= Objectifs --------- - lancer le RatBot complet en **simulation**, sans matériel ; - explorer un système ROS 2 réel : nœuds, topics, repères ; - écrire un **nœud ROS 2 en Python** qui commande le robot et lit son odométrie. Durée indicative : deux séances de 3 h. Prérequis : :doc:`tp02_recevoir` (topics, éditeurs, abonnés) ; des bases de Python. Matériel : un ordinateur sous Ubuntu 22.04 avec ROS 2 Humble et le dépôt du RatBot installé, ou le conteneur Docker du projet. L'installation est décrite dans le guide utilisateur : `Installer le PC `_. Ce qu'il faut savoir -------------------- Du TP 3 au robot ~~~~~~~~~~~~~~~~ Le robot complet, c'est la carte des TP précédents, plus un Raspberry Pi, deux roues, un lidar. Sa carte publie, exactement comme vous l'avez fait, la température, la batterie, la centrale inertielle ; elle reçoit les consignes de vitesse et commande les roues. Le Raspberry Pi y ajoute l'arbitrage des commandes, la fusion de l'odométrie, le lidar. La **simulation** remplace le robot par un modèle physique dans **Gazebo**, qui publie et reçoit **les mêmes topics** : un programme écrit pour la simulation marche sur le vrai robot. Les noms ~~~~~~~~ Tous les topics du robot *N* sont rangés dans l'espace de noms ``/ratbot_N`` : ``/ratbot_1/base/cmd_vel``, ``/ratbot_1/scan``… Les principaux sont décrits dans le guide : `Interface ROS `_. .. tikz:: La chaîne de commande du robot : trois sources, un arbitre, deux protections. \tikzset{n/.style={draw, rounded corners, fill=blue!7, align=center, font=\small, minimum height=10mm, inner sep=4pt}, ar/.style={-{Stealth}, thick}, lb/.style={font=\scriptsize, align=center}} \node[n, fill=black!4] (k) at (0,1.5) {clavier, manette\\[-2pt]\scriptsize cmd\_vel\_teleop, priorité 100}; \node[n, fill=black!4] (p) at (0,0) {vos programmes\\[-2pt]\scriptsize cmd\_vel, priorité 50}; \node[n, fill=black!4] (v) at (0,-1.5) {navigation (Nav2)\\[-2pt]\scriptsize cmd\_vel\_nav, priorité 10}; \node[n] (m) at (4.6,0) {twist\_mux\\[-2pt]\scriptsize la source active\\[-2pt]\scriptsize la plus prioritaire}; \node[n] (l) at (7.8,0) {limiteur\\[-2pt]\scriptsize 0,18 m/s\\[-2pt]\scriptsize 1,5 rad/s}; \node[n] (g) at (10.8,0) {garde-\\barrière\\[-2pt]\scriptsize arrêt de flotte}; \node[n, fill=orange!12] (c) at (13.8,0) {carte du robot\\[-2pt]\scriptsize ou Gazebo}; \foreach \s in {k,p,v} \draw[ar] (\s.east) -- (m.west); \draw[ar] (m) -- (l); \draw[ar] (l) -- (g); \draw[ar] (g) -- (c); \node[lb, anchor=north] at (4.6,-1.0) {source inactive\\0,5 s après son\\dernier message}; Le domaine, encore ~~~~~~~~~~~~~~~~~~ Comme au TP 1, chaque poste travaille dans **son** domaine, sinon toutes les simulations de la salle se mélangent. Dans **chaque** terminal : .. code-block:: bash export ROS_DOMAIN_ID=11 # 10 + numéro du poste (Ajoutez la ligne à ``~/.bashrc`` pour ne plus l'oublier.) Jamais 77 : c'est celui des robots, et ``ratbot up`` refuse d'y lancer une simulation si un robot répond. Étape 1 — Lancer et conduire ---------------------------- #. Depuis la racine du dépôt : .. code-block:: bash ./scripts/ratbot up rviz:=true .. Capture d'écran à insérer : Gazebo et RViz côte à côte, le robot dans le décor du laboratoire. Déposez _static/captures/ros/tp04-gazebo-rviz.png puis décommentez le bloc ci-dessous (retirez le préfixe « .. » devant chaque ligne) : .. .. image:: ../_static/captures/ros/tp04-gazebo-rviz.png .. :align: center .. :width: 90% Gazebo montre le robot dans le décor du laboratoire, RViz ce que le robot **perçoit**. #. Dans un second terminal, conduisez-le au clavier : .. code-block:: bash ./scripts/ratbot key 1 #. Dans un troisième, explorez : .. code-block:: bash ros2 node list ros2 topic list ros2 topic info /ratbot_1/base/cmd_vel -v ros2 topic hz /ratbot_1/scan ros2 topic echo /ratbot_1/odometry/filtered --field pose.pose.position ros2 run rqt_graph rqt_graph .. Capture d'écran à insérer : rqt_graph : la chaîne clavier → twist_mux → limiteur → garde-barrière → base. Déposez _static/captures/ros/tp04-rqt-graph.png puis décommentez le bloc ci-dessous (retirez le préfixe « .. » devant chaque ligne) : .. .. image:: ../_static/captures/ros/tp04-rqt-graph.png .. :align: center .. :width: 90% .. admonition:: Questions 1. Quels nœuds publient sur ``/ratbot_1/base/cmd_vel_teleop`` ? Lesquels s'y abonnent ? Retrouvez dans ``rqt_graph`` la chaîne de commande décrite dans l'Interface ROS du guide. 2. Quelle est la cadence du lidar ? de l'odométrie ? 3. Dans RViz, ajoutez l'affichage **TF**. Quels repères voyez-vous ? Que représente ``base_link`` ? ``odom`` ? .. dropdown:: Réponses aux questions 1 à 3 :icon: light-bulb :color: info **1. La chaîne de commande.** ``ros2 topic info /ratbot_1/base/cmd_vel_teleop -v`` montre que le **nœud de téléopération** (le clavier) y publie, et que **twist_mux** s'y abonne. ``rqt_graph`` montre la suite : twist_mux reçoit aussi ``base/cmd_vel`` et ``base/cmd_vel_nav``, choisit la source active la plus prioritaire, et publie vers le **limiteur** de vitesse, puis le **garde-barrière** (l'arrêt de flotte), puis la base (en simulation, le modèle Gazebo ; sur le robot, la carte). Chaque étage est un nœud séparé, qu'on peut observer, remplacer ou tester seul : c'est l'esprit de ROS. **2. Les cadences.** ``ros2 topic hz`` : le lidar publie à environ **10 Hz** (un tour de mesure par publication, comme le vrai lidar) ; l'odométrie fusionnée ``odometry/filtered``, produite par un filtre de Kalman (EKF) qui combine roues et gyroscope, à **30 Hz**. Ces cadences dimensionnent tout le reste : un algorithme qui utilise le lidar ne peut pas réagir plus vite que 10 fois par seconde. **3. Les repères.** On voit notamment ``ratbot_1/odom``, ``ratbot_1/base_footprint``, ``ratbot_1/base_link``, les roues, le lidar, la centrale, et, en simulation, ``world``. - ``base_link`` est le repère **du robot** : il bouge avec lui, et tous ses capteurs y sont fixés à une position connue (le modèle URDF). C'est ce qui permet de ramener une mesure du lidar dans le repère du robot. - ``odom`` est un repère **fixe**, posé là où le robot a démarré : l'odométrie y accumule ses déplacements. La transformation ``odom → base_link`` *est* la position estimée du robot. Étape 2 — Un premier nœud Python : avancer ------------------------------------------ Écrivez ``avance.py`` : .. code-block:: python import rclpy from rclpy.node import Node from geometry_msgs.msg import Twist class Avance(Node): def __init__(self): super().__init__('ratbot_lab_avance') self.pub = self.create_publisher(Twist, '/ratbot_1/base/cmd_vel', 10) self.timer = self.create_timer(0.05, self.tick) # 20 Hz def tick(self): cmd = Twist() cmd.linear.x = 0.1 # m/s self.pub.publish(cmd) def main(): rclpy.init() rclpy.spin(Avance()) if __name__ == '__main__': main() Lancez-le avec ``python3 avance.py``, puis arrêtez-le (Ctrl+C). .. admonition:: Questions 4. Que fait le robot quand on arrête le programme ? Au bout de combien de temps ? Qui décide de l'arrêter ? (Voir le guide.) 5. Pendant que ``avance.py`` tourne, conduisez au clavier. Qui l'emporte ? Pourquoi ? 6. Remplacez 0,1 par 1,0 m/s. Quelle vitesse mesurez-vous dans ``odometry/filtered`` ? Pourquoi ? .. dropdown:: Réponses aux questions 4 à 6 :icon: light-bulb :color: info **4. À l'arrêt du programme.** Le robot s'arrête au bout d'environ **une demi-seconde**. Personne ne lui a envoyé de consigne nulle : c'est **twist_mux** qui considère une source comme inactive 0,5 s après son dernier message (et, sur le vrai robot, la carte coupe aussi les moteurs 0,5 s après la dernière commande reçue). C'est une sécurité : un programme qui plante, une liaison qui tombe, et le robot s'arrête de lui-même. La contrepartie : un programme doit **republier** sa consigne régulièrement, d'où le timer à 20 Hz. **5. Le clavier et le programme.** Le **clavier** l'emporte : il publie sur ``cmd_vel_teleop`` (priorité 100), le programme sur ``cmd_vel`` (priorité 50). twist_mux ne transmet que la source active la plus prioritaire ; quand on lâche le clavier, il repasse au programme après 0,5 s. Un humain peut toujours reprendre la main sur un programme ou sur la navigation, sans rien arrêter. **6. À 1 m/s.** On mesure environ **0,18 m/s** dans ``odometry/filtered`` : le **limiteur** plafonne la vitesse à 0,18 m/s (et 1,5 rad/s), quelle que soit la demande. Le programme demande, la chaîne décide. Les limites physiques et de sécurité du robot ne sont pas dans chaque programme, mais en un seul endroit, qu'aucun programme ne peut contourner. Étape 3 — Un carré, mesuré -------------------------- Écrivez ``carre.py`` : le robot décrit un **carré de 0,5 m de côté**, en mesurant ses déplacements avec l'odométrie, pas avec le temps. #. Abonnez-vous à ``/ratbot_1/odometry/filtered`` (``nav_msgs/msg/Odometry``) et gardez la dernière pose : position ``x``, ``y``, et le **cap**, calculé à partir du quaternion de l'orientation : .. code-block:: python import math q = msg.pose.pose.orientation cap = math.atan2(2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z)) #. Une **machine à états** dans le timer : AVANCER jusqu'à 0,5 m du point de départ du côté, puis TOURNER jusqu'à 90° du cap de départ, quatre fois, puis FINI (vitesse nulle, publiée une fois). #. Ralentissez à l'approche de chaque but, pour ne pas le dépasser. Faites-le tourner en simulation, et comparez le point d'arrivée au point de départ : dans l'odométrie, et dans Gazebo, qui montre la position réelle du modèle. .. Capture d'écran à insérer : RViz : la trajectoire du robot pendant le carré (affichage Odometry ou Path). Déposez _static/captures/ros/tp04-carre.png puis décommentez le bloc ci-dessous (retirez le préfixe « .. » devant chaque ligne) : .. .. image:: ../_static/captures/ros/tp04-carre.png .. :align: center .. :width: 90% .. admonition:: Questions 7. Pourquoi mesurer la distance parcourue plutôt que d'avancer pendant 5 s à 0,1 m/s ? 8. Le calcul d'angle doit gérer le passage de +180° à −180°. Comment ? 9. Le robot revient-il exactement à son point de départ ? D'où vient l'écart ? Que donnerait le même programme sur le vrai robot ? .. dropdown:: Solution de l'étape 3 :icon: code :color: secondary L'essentiel de ``carre.py`` (le fichier complet est ci-dessous) : .. code-block:: python V_MAX = 0.15 # m/s, sous le plafond du limiteur W_MAX = 0.8 # rad/s TOL_D = 0.01 # m TOL_A = math.radians(1.0) def yaw_of(q): return math.atan2(2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z)) def wrap(a): """Ramène un angle dans ]-pi, pi].""" return math.atan2(math.sin(a), math.cos(a)) class Carre(Node): def __init__(self, robot, side): super().__init__('ratbot_lab_carre') ns = f'/ratbot_{robot}' self.side = side self.pub = self.create_publisher(Twist, f'{ns}/base/cmd_vel', 10) self.create_subscription(Odometry, f'{ns}/odometry/filtered', self.on_odom, 10) self.pose = None self.state = 'ATTENTE' self.sides_done = 0 self.start = None # pose au début du côté en cours self.origin = None # pose de départ du carré self.timer = self.create_timer(0.05, self.tick) def on_odom(self, msg): p = msg.pose.pose self.pose = (p.position.x, p.position.y, yaw_of(p.orientation)) def tick(self): if self.pose is None: return # pas encore d'odométrie x, y, a = self.pose if self.state == 'ATTENTE': self.origin = self.start = self.pose self.state = 'AVANCER' if self.state == 'AVANCER': reste = self.side - math.hypot(x - self.start[0], y - self.start[1]) if reste <= TOL_D: self.start = self.pose self.state = 'TOURNER' self.send(0.0, 0.0) return self.send(max(0.03, min(V_MAX, 1.0 * reste)), 0.0) # ralentit à l'approche elif self.state == 'TOURNER': but = wrap(self.origin[2] + (self.sides_done + 1) * math.pi / 2) reste = wrap(but - a) if abs(reste) <= TOL_A: self.sides_done += 1 self.start = self.pose self.state = 'FINI' if self.sides_done == 4 else 'AVANCER' self.send(0.0, 0.0) return w = max(0.15, min(W_MAX, 2.0 * abs(reste))) self.send(0.0, math.copysign(w, reste)) En simulation, le carré de 0,5 m se referme à **0,4 cm** et **−0,6°** près, d'après l'odométrie. **Le cap à partir du quaternion.** ROS représente une orientation en 3D par un **quaternion** (x, y, z, w), sans ambiguïté ni singularité, mais peu lisible. Pour un robot qui roule à plat, seule compte la rotation autour de l'axe vertical : la formule extrait cet angle (le *lacet*, *yaw*), entre −π et π. **Un régulateur proportionnel.** La vitesse est proportionnelle à ce qu'il reste à parcourir (``1.0 * reste``), bornée en haut (``V_MAX``) et en bas (0,03 m/s, sans quoi le robot n'arriverait jamais : la vitesse tendrait vers zéro avec l'écart). Le robot ralentit en approchant du but, et le dépasse à peine. **Des buts absolus pour les virages.** Chaque virage vise ``origine + k × 90°``, et non « le cap actuel + 90° » : les petites erreurs d'arrêt de chaque virage (jusqu'à 1°) ne s'additionnent pas. Avec des buts relatifs, on mesurait −2,5° d'erreur finale au lieu de −0,6°. **Lancer** : ``python3 carre.py`` (robot 1, côté 0,5 m), ou ``python3 carre.py 2 1.0`` (robot 2, côté 1 m). .. dropdown:: Réponses aux questions 7 à 9 :icon: light-bulb :color: info **7. Mesurer plutôt que chronométrer.** « 5 s à 0,1 m/s » suppose que le robot roule **exactement** à la vitesse demandée **dès** la première seconde. En réalité : il accélère et freine (rampe de la carte), la commande met un peu de temps à arriver, le limiteur peut réduire la vitesse, les roues peuvent patiner, la batterie qui faiblit change la vitesse réelle. Le temps ne mesure **rien** de ce qui s'est passé ; l'odométrie, si. C'est la différence entre une commande **en boucle ouverte** et une commande **en boucle fermée**, qui corrige d'après la mesure. **8. Le passage de +180° à −180°.** ``atan2`` donne un angle dans ]−π, π]. Si le robot est à 179° et vise −179°, la différence brute vaut −358° : il ferait presque un tour complet dans le mauvais sens. On ramène toute **différence** d'angles dans ]−π, π] : ``atan2(sin(d), cos(d))``. −358° devient +2° : le robot tourne de 2° dans le bon sens. Toute comparaison d'angles doit passer par là. **9. Le retour au départ.** Presque, mais pas exactement : les tolérances d'arrêt (1 cm, 1°), l'inertie, et surtout la **dérive de l'odométrie**. L'odométrie de la simulation est presque parfaite (pas de glissement, roues idéales) : le carré se referme à quelques millimètres. Sur le **vrai robot**, l'écart serait de plusieurs centimètres : rayons de roue un peu différents, entraxe mal connu, glissement au démarrage et dans les virages. Et le robot, mesurant avec cette même odométrie, **croirait** être revenu. C'est exactement ce que mesure l'étalonnage UMBmark du robot (un carré dans les deux sens), et ce qu'on corrige avec le lidar (TP 5). .. dropdown:: Tout le code de la correction :icon: download :color: secondary :download:`carre.py <../../corrections/ros/tp04/carre.py>` — essayé en simulation (``ratbot up``, robot 1). Il se lance directement, après ``source /opt/ros/humble/setup.bash`` et ``export ROS_DOMAIN_ID=…`` : ``python3 carre.py [N] [CÔTÉ]``. Exercices supplémentaires ------------------------- Ces exercices n'ont pas de correction. - **Un paquet** : rangez vos nœuds dans un paquet ROS 2 Python (``ros2 pkg create --build-type ament_python ratbot_lab_tp``), et lancez-les avec ``ros2 run``. Le côté du carré devient un **paramètre**. - **L'évitement** : un nœud qui avance tout droit et s'arrête si le lidar (``/ratbot_1/scan``) voit un obstacle à moins de 30 cm devant. - **Suivre un mur** : le robot longe un mur à 25 cm, à l'aide des mesures latérales du lidar (un régulateur proportionnel suffit). Après le TP ----------- Arrêtez la simulation (Ctrl+C dans le terminal de ``ratbot up``).