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 : TP 5 — Cartographier et naviguer.

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 :

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.

Figure made with TikZ

L’arbre des repères avec deux robots : en simulation, world relie les deux arbres.

Étape 1 — Deux robots, un tableau de bord

  1. Lancez deux robots :

    ./scripts/ratbot up rooms 2 rviz:=true

  2. Explorez : ros2 node list, ros2 topic list, et l’arbre des repères (ros2 run rqt_tf_tree rqt_tf_tree).

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

    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

  4. Conduisez un robot au clavier (ratbot key 1), puis l’autre.

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 ?

Solution de l’étape 1

L’essentiel de flotte.py :

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.

Réponses aux questions 1 à 4

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.

  1. Un TransformListener remplit un Buffer :

    from tf2_ros import Buffer, TransformListener
self.tf = Buffer()
self.listener = TransformListener(self.tf, self)

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

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

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

Questions

  1. Le robot 1 revient vers le robot 2. Que se passe-t-il ? Comment éviter la collision ?

  2. Ce suiveur marcherait-il sur deux vrais robots ? Que faudrait-il pour que chacun connaisse la position de l’autre ?

Solution de l’étape 2

L’essentiel de suiveur.py :

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.

Réponses aux questions 5 et 6

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.

Tout le code de la correction

flotte.py et 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.