TP 6 — Plusieurs robots ======================= Objectifs --------- - faire cohabiter plusieurs robots dans un même système ROS 2 : **espaces de noms**, **repères** préfixés ; - écrire un nœud qui **découvre** les robots présents et les surveille ; - faire **suivre** un robot par un autre, à l'aide des transformations entre repères (**TF**). Durée indicative : deux séances de 3 h. Prérequis : :doc:`tp05_carte_navigation`. Ce qu'il faut savoir -------------------- Un nom par robot ~~~~~~~~~~~~~~~~ Deux robots identiques publient les mêmes topics. Pour qu'ils ne se mélangent pas, chacun range tout ce qu'il publie dans **son espace de noms** : ``/ratbot_1/scan``, ``/ratbot_2/scan``. Les **repères** sont préfixés de la même façon : ``ratbot_1/base_footprint``, ``ratbot_2/base_footprint``. Le guide le détaille : `Noms et repères `_ et `La flotte `_. Les transformations entre repères ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ **TF** tient à jour l'arbre des repères : chaque lien donne la position d'un repère dans son parent. Un programme peut demander la position de **n'importe quel** repère dans **n'importe quel** autre, pourvu qu'ils soient dans le même arbre : .. code-block:: python t = self.tf.lookup_transform('ratbot_2/base_footprint', # dans ce repère... 'ratbot_1/base_footprint', # ...où est celui-ci ? Time()) En simulation, les arbres des robots sont reliés par un repère commun, ``world`` : la **vérité terrain**, que seul un simulateur connaît. .. tikz:: L'arbre des repères avec deux robots : en simulation, world relie les deux arbres. \tikzset{f/.style={draw, rounded corners, fill=blue!7, font=\small\ttfamily, inner sep=4pt}, ar/.style={-{Stealth}, thick}, lb/.style={font=\scriptsize, align=center}} \node[f, fill=orange!14] (w) at (0,0) {world}; \foreach \r/\x in {1/-4.6, 2/4.6} { \node[f] (o\r) at (\x,-1.5) {ratbot\_\r/odom}; \node[f] (b\r) at (\x,-3.0) {ratbot\_\r/base\_footprint}; \node[f] (l\r) at (\x,-4.5) {…/base\_link}; \node[f] (s\r) at (\x-1.4,-6.0) {…/laser\_link}; \node[f] (i\r) at (\x+1.4,-6.0) {…/imu\_link}; \draw[ar] (w) -- node[lb, sloped, above]{vérité terrain} (o\r); \draw[ar] (o\r) -- node[lb, right]{odométrie} (b\r); \draw[ar] (b\r) -- (l\r); \draw[ar] (l\r) -- (s\r); \draw[ar] (l\r) -- (i\r); } \draw[red!70!black, thick, dashed, {Stealth}-{Stealth}] (b1.east) -- node[lb, above]{lookup\_transform} node[lb, below]{passe par world} (b2.west); Étape 1 — Deux robots, un tableau de bord ----------------------------------------- #. Lancez deux robots : .. code-block:: bash ./scripts/ratbot up rooms 2 rviz:=true .. Capture d'écran à insérer : Gazebo avec les deux robots dans le décor rooms. Déposez _static/captures/ros/tp06-deux-robots.png puis décommentez le bloc ci-dessous (retirez le préfixe « .. » devant chaque ligne) : .. .. image:: ../_static/captures/ros/tp06-deux-robots.png .. :align: center .. :width: 90% #. Explorez : ``ros2 node list``, ``ros2 topic list``, et l'arbre des repères (``ros2 run rqt_tf_tree rqt_tf_tree``). #. Écrivez ``flotte.py`` : toutes les deux secondes, il **découvre** les robots présents à partir des noms de topics (``self.get_topic_names_and_types()``, noms commençant par ``/ratbot_N/``), s'abonne à la batterie (``sensors/battery``) et à l'odométrie de chaque **nouveau** robot, et affiche un tableau : .. Capture d'écran à insérer : rqt_tf_tree : world, et les deux arbres ratbot_1 et ratbot_2. Déposez _static/captures/ros/tp06-tf-tree.png puis décommentez le bloc ci-dessous (retirez le préfixe « .. » devant chaque ligne) : .. .. image:: ../_static/captures/ros/tp06-tf-tree.png .. :align: center .. :width: 90% .. code-block:: text robot batterie x (odom) y (odom) vitesse ratbot_1 12.57 V 0.39 0.00 0.11 m/s ratbot_2 12.56 V 0.00 0.00 0.00 m/s #. Conduisez un robot au clavier (``ratbot key 1``), puis l'autre. .. admonition:: Questions 1. Comment se nomment les topics et les nœuds des deux robots ? 2. Combien de topics ``/tf`` y a-t-il ? Comment les repères des deux robots se distinguent-ils ? 3. Quel repère relie les deux arbres ? Que se passe-t-il avec ``ground_truth_tf:=false`` ? 4. Les deux robots affichent ``x = 0, y = 0`` au départ, alors qu'ils ne sont pas au même endroit. Pourquoi ? .. dropdown:: Solution de l'étape 1 :icon: code :color: secondary L'essentiel de ``flotte.py`` : .. code-block:: python PATTERN = re.compile(r'^/(ratbot_\d+)/') class Flotte(Node): def __init__(self): super().__init__('ratbot_lab_flotte') self.robots = {} # nom -> dernier état connu self.subs = [] # garder les abonnements en vie self.create_timer(2.0, self.tick) def discover(self): for name, _types in self.get_topic_names_and_types(): m = PATTERN.match(name) if m and m.group(1) not in self.robots: self.add(m.group(1)) def add(self, robot): state = {'v': float('nan'), 'x': float('nan'), 'y': float('nan'), 'speed': 0.0} self.robots[robot] = state def on_batt(msg, s=state): s['v'] = msg.voltage def on_odom(msg, s=state): s['x'] = msg.pose.pose.position.x s['y'] = msg.pose.pose.position.y s['speed'] = msg.twist.twist.linear.x self.subs.append(self.create_subscription( BatteryState, f'/{robot}/sensors/battery', on_batt, 10)) self.subs.append(self.create_subscription( Odometry, f'/{robot}/odometry/filtered', on_odom, 10)) def tick(self): self.discover() # … affiche une ligne par robot, triés par numéro … **La découverte.** ``get_topic_names_and_types`` interroge le **graphe ROS** : la liste de tous les topics connus, publiés par n'importe quel nœud du domaine. Une expression régulière en extrait les noms de robots. Le tableau suit ainsi les robots qui **apparaissent** en cours de route, sans configuration : c'est aussi ce que fait ``ratbot fleet status``. **Le piège des fonctions de rappel en boucle.** ``s=state`` dans la signature fige, pour chaque rappel, **le** dictionnaire de son robot. Sans cela, une fonction définie dans une boucle verrait la **dernière** valeur de la variable : tous les rappels mettraient à jour le même robot. **Garder les abonnements.** ``create_subscription`` renvoie un objet ; s'il n'est gardé nulle part, Python peut le détruire, et l'abonnement disparaît. La liste ``self.subs`` les garde en vie. .. dropdown:: Réponses aux questions 1 à 4 :icon: light-bulb :color: info **1. Les noms.** Tout ce qui appartient au robot *N* est rangé sous ``/ratbot_N`` : les topics (``/ratbot_1/scan``, ``/ratbot_2/scan``…), et les nœuds aussi (``/ratbot_1/twist_mux``, ``/ratbot_2/twist_mux``…). Les deux robots lancent exactement les mêmes programmes, avec un **espace de noms** différent : aucun nom n'entre en conflit. **2. Les topics /tf.** Il n'y a **qu'un** topic ``/tf`` (et un ``/tf_static``) pour tout le système : TF est un arbre unique, partagé par tous. Les repères se distinguent par leur **préfixe** : ``ratbot_1/odom``, ``ratbot_1/base_footprint``, ``ratbot_2/odom``… Sans préfixe, les deux robots publieraient chacun un repère « base_footprint » : deux positions pour un même nom, et un arbre incohérent. **3. Le repère commun.** C'est **world**, publié par la simulation (la vérité terrain) : ``world → ratbot_1/odom`` et ``world → ratbot_2/odom``, à la position de départ de chaque robot. Avec ``ground_truth_tf:=false``, ces liens disparaissent : on a **deux arbres séparés**, et aucune transformation ne permet de passer de l'un à l'autre. Un robot ne sait plus où est l'autre. **4. Deux robots à (0, 0).** Chaque odométrie part de **zéro**, à l'endroit où **son** robot a démarré : chaque repère ``odom`` est posé à la place de départ de son robot. Les deux « (0, 0) » désignent deux points différents du monde. Pour comparer les positions de deux robots, il faut un **repère commun** : ``world`` en simulation, la carte (``map``) en navigation. Étape 2 — Le suiveur -------------------- Écrivez ``suiveur.py`` : le robot 2 suit le robot 1, à 0,6 m. #. Un ``TransformListener`` remplit un ``Buffer`` : .. code-block:: python from tf2_ros import Buffer, TransformListener self.tf = Buffer() self.listener = TransformListener(self.tf, self) #. À 20 Hz : demandez la position du robot 1 **dans le repère du robot 2**. Vous obtenez ``x`` (devant) et ``y`` (à gauche). Calculez la distance et l'angle ``atan2(y, x)``. #. Commande : tourner vers le meneur (proportionnel à l'angle), et n'avancer que s'il est à peu près devant, d'autant plus vite que l'écart dépasse 0,6 m. Bornez les vitesses. Publiez sur ``/ratbot_2/base/cmd_vel``. #. Si la transformation n'est pas disponible (``tf2_ros.TransformException``), arrêtez le robot. Conduisez le robot 1 au clavier, ou faites-lui faire le carré du TP 4. .. admonition:: Questions 5. Le robot 1 revient vers le robot 2. Que se passe-t-il ? Comment éviter la collision ? 6. Ce suiveur marcherait-il sur deux vrais robots ? Que faudrait-il pour que chacun connaisse la position de l'autre ? .. dropdown:: Solution de l'étape 2 :icon: code :color: secondary L'essentiel de ``suiveur.py`` : .. code-block:: python from tf2_ros import Buffer, TransformListener, TransformException class Suiveur(Node): def __init__(self, leader, follower, distance): super().__init__('ratbot_lab_suiveur') self.leader = f'ratbot_{leader}/base_footprint' self.follower = f'ratbot_{follower}/base_footprint' self.distance = distance self.pub = self.create_publisher(Twist, f'/ratbot_{follower}/base/cmd_vel', 10) self.tf = Buffer() self.listener = TransformListener(self.tf, self) self.create_timer(0.05, self.tick) def tick(self): cmd = Twist() try: # position du meneur dans le repère du suiveur t = self.tf.lookup_transform(self.follower, self.leader, Time()) except TransformException: self.pub.publish(cmd) # pas de position : on s'arrête return x = t.transform.translation.x # devant (+) / derrière (-) y = t.transform.translation.y # à gauche (+) / à droite (-) dist = math.hypot(x, y) angle = math.atan2(y, x) cmd.angular.z = max(-W_MAX, min(W_MAX, 1.5 * angle)) if abs(angle) < math.radians(45): # n'avance que s'il est devant cmd.linear.x = max(0.0, min(V_MAX, 0.8 * (dist - self.distance))) self.pub.publish(cmd) En simulation, le suiveur se stabilise vers 0,5 m derrière son meneur. **Ce que fait lookup_transform.** Le ``TransformListener`` écoute ``/tf`` et remplit le ``Buffer`` avec toutes les transformations reçues, horodatées. ``lookup_transform(cible, source, instant)`` remonte l'arbre de ``source`` jusqu'à l'ancêtre commun (ici ``world``), redescend jusqu'à ``cible``, et compose les transformations : on obtient la position du meneur **vue depuis le suiveur**, directement. ``Time()`` (l'instant zéro) veut dire « la plus récente disponible ». **La commande.** Dans le repère du suiveur, ``x`` est la distance devant lui, ``y`` à sa gauche : ``atan2(y, x)`` est l'angle dont il doit tourner pour faire face au meneur. Deux régulateurs proportionnels : l'un tourne vers le meneur, l'autre avance d'autant plus vite que l'écart dépasse la distance voulue. N'avancer que si le meneur est devant (à moins de 45°) évite de partir en arc de cercle quand il est derrière. **En cas d'échec de TF**, on publie une consigne **nulle** : sans information, le robot doit s'arrêter, pas continuer sur sa dernière commande. .. dropdown:: Réponses aux questions 5 et 6 :icon: light-bulb :color: info **5. Le meneur qui revient.** Le suiveur **ne recule jamais** : sa vitesse est bornée à zéro en bas (``max(0.0, …)``). Si le meneur vient vers lui, l'écart diminue sans qu'il réagisse, sauf à tourner pour lui faire face ; en simulation, l'écart est descendu jusqu'à **0,11 m** pendant que le meneur faisait le carré du TP 4. Pour éviter la collision : - autoriser une vitesse **négative** (reculer) quand l'écart est sous la distance voulue ; - ou ajouter un **arrêt de sécurité** : sous 0,3 m, vitesse nulle, et surtout pas de rotation qui balaierait l'autre robot ; - et, sur le vrai robot, compter sur l'évitement de la navigation, qui voit l'autre robot avec son lidar. **6. Sur de vrais robots.** Pas tel quel : la vérité terrain n'existe pas dans le monde réel. Chaque robot ne connaît que sa position dans **son** ``odom``. Pour qu'un robot sache où est l'autre, il faut un **repère commun** : - les deux robots se **localisent dans la même carte** (AMCL, repère ``map``) et **publient** leur position ; le suiveur compose alors les deux positions ; - ou un système **extérieur** les mesure tous les deux : des caméras au plafond, des balises radio (UWB), un système de capture de mouvement ; - ou le suiveur **voit** le meneur directement, avec son lidar ou une caméra (une cible à reconnaître). C'est le cœur du travail en flotte : partager un repère, et des positions dans ce repère. .. dropdown:: Tout le code de la correction :icon: download :color: secondary :download:`flotte.py <../../corrections/ros/tp06/flotte.py>` et :download:`suiveur.py <../../corrections/ros/tp06/suiveur.py>` — essayés en simulation (``ratbot up rooms 2``). Ils se lancent directement, après ``source /opt/ros/humble/setup.bash`` et ``export ROS_DOMAIN_ID=…`` : ``python3 flotte.py`` et ``python3 suiveur.py [MENEUR] [SUIVEUR] [DISTANCE]``. Exercices supplémentaires ------------------------- Ces exercices n'ont pas de correction. - **La file indienne** : quatre robots (``ratbot up hampton_simple 4``), chacun suit le précédent. - **L'alerte batterie** : ``flotte.py`` publie un message de diagnostic (``/diagnostics``) quand la tension d'un robot passe sous 10,5 V. - **Deux robots, une carte** : cartographiez avec le robot 1, puis lancez la navigation des deux robots sur cette carte, et envoyez-les à deux buts différents en même temps. - **Le vrai parc** : avec l'enseignant, sur les robots réels (domaine 77), lancez ``ratbot fleet status`` et comparez avec votre ``flotte.py``. Après le TP ----------- Arrêtez vos programmes, puis la simulation.