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 : TP 2 — Recevoir des ordres (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.

Figure made with TikZ

La chaîne de commande du robot : trois sources, un arbitre, deux protections.

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 :

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 :
```
./scripts/ratbot up rviz:=true
```
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 :
```
./scripts/ratbot key 1
```

Dans un troisième, explorez :

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

Questions

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.
Quelle est la cadence du lidar ? de l’odométrie ?
Dans RViz, ajoutez l’affichage TF. Quels repères voyez-vous ? Que représente base_link ? odom ?

Étape 2 — Un premier nœud Python : avancer

Écrivez avance.py :

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

Questions

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.)
Pendant que avance.py tourne, conduisez au clavier. Qui l’emporte ? Pourquoi ?
Remplacez 0,1 par 1,0 m/s. Quelle vitesse mesurez-vous dans odometry/filtered ? Pourquoi ?

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

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.

Questions

Pourquoi mesurer la distance parcourue plutôt que d’avancer pendant 5 s à 0,1 m/s ?
Le calcul d’angle doit gérer le passage de +180° à −180°. Comment ?
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 ?

Solution de l’étape 3

L’essentiel de carre.py (le fichier complet est ci-dessous) :

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

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