Écrire un diffuseur (Python)

Objectif : Apprendre à diffuser l’état d’un robot sur TF2.

Niveau du didacticiel : Intermédiaire

Durée : 15 minutes

Arrière-plan

Dans les deux prochains tutoriels, nous écrirons le code pour reproduire la démo du tutoriel Introduction to tf2. Après cela, les didacticiels suivants se concentrent sur l’extension de la démo avec des fonctionnalités tf2 plus avancées, y compris l’utilisation de délais d’attente dans les recherches de transformation et le voyage dans le temps.

Conditions préalables

Ce tutoriel suppose que vous avez une connaissance pratique de ROS 2 et que vous avez terminé le Tutoriel d’introduction à tf2. Dans les tutoriels précédents, vous avez appris à créer un espace de travail et créer un package. Vous avez également créé le learning_tf2_py package, à partir duquel nous continuerons à travailler.

Tâches

1 Écrivez le nœud diffuseur

Commençons par créer les fichiers source. Accédez au package learning_tf2_py que nous avons créé dans le tutoriel précédent. Dans le répertoire src/learning_tf2_py/learning_tf2_py, téléchargez l’exemple de code de diffuseur en saisissant la commande suivante :

LinuxmacOSWindows

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_broadcaster.py

Ouvrez le fichier à l’aide de votre éditeur de texte préféré.

import math

from geometry_msgs.msg import TransformStamped

import numpy as np

import rclpy
from rclpy.node import Node

from tf2_ros import TransformBroadcaster

from turtlesim.msg import Pose


def quaternion_from_euler(ai, aj, ak):
    ai /= 2.0
    aj /= 2.0
    ak /= 2.0
    ci = math.cos(ai)
    si = math.sin(ai)
    cj = math.cos(aj)
    sj = math.sin(aj)
    ck = math.cos(ak)
    sk = math.sin(ak)
    cc = ci*ck
    cs = ci*sk
    sc = si*ck
    ss = si*sk

    q = np.empty((4, ))
    q[0] = cj*sc - sj*cs
    q[1] = cj*ss + sj*cc
    q[2] = cj*cs - sj*sc
    q[3] = cj*cc + sj*ss

    return q


class FramePublisher(Node):

    def __init__(self):
        super().__init__('turtle_tf2_frame_publisher')

        # Declare and acquire `turtlename` parameter
        self.turtlename = self.declare_parameter(
          'turtlename', 'turtle').get_parameter_value().string_value

        # Initialize the transform broadcaster
        self.tf_broadcaster = TransformBroadcaster(self)

        # Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
        # callback function on each message
        self.subscription = self.create_subscription(
            Pose,
            f'/{self.turtlename}/pose',
            self.handle_turtle_pose,
            1)
        self.subscription  # prevent unused variable warning

    def handle_turtle_pose(self, msg):
        t = TransformStamped()

        # Read message content and assign it to
        # corresponding tf variables
        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'world'
        t.child_frame_id = self.turtlename

        # Turtle only exists in 2D, thus we get x and y translation
        # coordinates from the message and set the z coordinate to 0
        t.transform.translation.x = msg.x
        t.transform.translation.y = msg.y
        t.transform.translation.z = 0.0

        # For the same reason, turtle can only rotate around one axis
        # and this why we set rotation in x and y to 0 and obtain
        # rotation in z axis from the message
        q = quaternion_from_euler(0, 0, msg.theta)
        t.transform.rotation.x = q[0]
        t.transform.rotation.y = q[1]
        t.transform.rotation.z = q[2]
        t.transform.rotation.w = q[3]

        # Send the transformation
        self.tf_broadcaster.sendTransform(t)


def main():
    rclpy.init()
    node = FramePublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

1.1 Examiner le code

Maintenant, regardons le code qui est pertinent pour publier la pose de la tortue sur tf2. Tout d’abord, nous définissons et acquérons un seul paramètre turtlename, qui spécifie un nom de tortue, par ex. tortue1 ou tortue2.

self.turtlename = self.declare_parameter(
  'turtlename', 'turtle').get_parameter_value().string_value

Ensuite, le nœud s’abonne au sujet turtleX/pose et exécute la fonction handle_turtle_pose sur chaque message entrant.

self .subscription = self.create_subscription(
    Pose,
    f'/{self.turtlename}/pose',
    self.handle_turtle_pose,
    1)

Maintenant, nous créons un objet TransformStamped et lui donnons les métadonnées appropriées.

1. Nous devons donner à la transformation en cours de publication un horodatage, et nous l’estampillerons simplement avec l’heure actuelle en appelant self.get_clock().now(). Cela renverra l’heure actuelle utilisée par le Node.

2. Ensuite, nous devons définir le nom du cadre parent du lien que nous créons, dans ce cas world.

3. Enfin, nous devons définir le nom du nœud enfant du lien que nous créons, dans ce cas, il s’agit du nom de la tortue elle-même.

La fonction de gestion du message de pose de tortue diffuse la translation et la rotation de cette tortue et la publie sous forme de transformation du cadre world au cadre turtleX.

t = TransformStamped()

# Read message content and assign it to
# corresponding tf variables
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'world'
t.child_frame_id = self.turtlename

Ici, nous copions les informations de la pose de la tortue 3D dans la transformation 3D.

# Turtle only exists in 2D, thus we get x and y translation
# coordinates from the message and set the z coordinate to 0
t.transform.translation.x = msg.x
t.transform.translation.y = msg.y
t.transform.translation.z = 0.0

# For the same reason, turtle can only rotate around one axis
# and this why we set rotation in x and y to 0 and obtain
# rotation in z axis from the message
q = quaternion_from_euler(0, 0, msg.theta)
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]

Enfin, nous prenons la transformation que nous avons construite et la passons à la méthode sendTransform du TransformBroadcaster qui s’occupera de la diffusion.

# Send the transformation
self.tf_broadcaster.sendTransform(t)

Note

Vous pouvez également publier des transformations statiques avec le même motif en instanciant un tf2_ros.StaticTransformBroadcaster au lieu d’un tf2_ros.TransformBroadcaster. Les transformations statiques seront publiées sur le sujet /tf_static et ne seront envoyées qu’en cas de besoin, pas périodiquement. Pour plus de détails, voir ici.

1.2 Ajouter un point d’entrée

Pour permettre à la commande ros2 run d’exécuter votre nœud, vous devez ajouter le point d’entrée à setup.py (situé dans le répertoire src/learning_tf2_py).

Enfin, ajoutez la ligne suivante entre les crochets 'console_scripts': :

'turtle_tf2_broadcaster = learning_tf2_py.turtle_tf2_broadcaster:main',

2 Écrivez le fichier de lancement

Créez maintenant un fichier de lancement pour cette démo. Avec votre éditeur de texte, créez un nouveau fichier appelé turtle_tf2_demo.launch.py dans le dossier launch, et ajoutez les lignes suivantes :

from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        Node(
            package='turtlesim',
            executable='turtlesim_node',
            name='sim'
        ),
        Node(
            package='learning_tf2_py',
            executable='turtle_tf2_broadcaster',
            name='broadcaster1',
            parameters=[
                {'turtlename': 'turtle1'}
            ]
        ),
    ])

2.1 Examiner le code

Nous importons d’abord les modules requis depuis les packages launch et launch_ros. Il convient de noter que launch est un cadre de lancement générique (non spécifique à ROS 2) et launch_ros a des éléments spécifiques à ROS 2, comme les nœuds que nous importons ici.

from launch import LaunchDescription
from launch_ros.actions import Node

Maintenant, nous exécutons nos nœuds qui démarrent la simulation turtlesim et diffusent l’état turtle1 au tf2 en utilisant notre nœud turtle_tf2_broadcaster.

Node(
    package='turtlesim',
    executable='turtlesim_node',
    name='sim'
),
Node(
    package='learning_tf2_py',
    executable='turtle_tf2_broadcaster',
    name='broadcaster1',
    parameters=[
        {'turtlename': 'turtle1'}
    ]
),

2.2 Ajouter des dépendances

Naviguez d’un niveau vers le répertoire src/learning_tf2_py, où se trouvent les fichiers setup.py, setup.cfg et package.xml.

Ouvrez package.xml avec votre éditeur de texte. Ajoutez les dépendances suivantes correspondant aux instructions d’importation de votre fichier de lancement :

<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>

Cela déclare les dépendances supplémentaires requises launch et launch_ros lorsque son code est exécuté.

Assurez-vous de sauvegarder le fichier.

2.3 Mettre à jour setup.py

Rouvrez setup.py et ajoutez la ligne pour que les fichiers de lancement du dossier launch/ soient installés. Le champ data_files devrait maintenant ressembler à ceci :

data_files=[
    ...
    (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
],

Ajoutez également les importations appropriées en haut du fichier :

import os
from glob import glob

Vous pouvez en savoir plus sur la création de fichiers de lancement dans ce tutoriel.

3 Construire

Exécutez rosdep à la racine de votre espace de travail pour vérifier les dépendances manquantes.

LinuxmacOSWindows

rosdep install -i --from-path src --rosdistro rolling -y

Toujours à la racine de votre espace de travail, construisez votre package :

LinuxmacOSWindows

colcon build --packages-select learning_tf2_py

Ouvrez un nouveau terminal, accédez à la racine de votre espace de travail et sourcez les fichiers de configuration :

LinuxmacOSWindows

. install/setup.bash

4 Courir

Exécutez maintenant le fichier de lancement qui lancera le nœud de simulation turtlesim et le nœud turtle_tf2_broadcaster :

ros2 launch learning_tf2_py turtle_tf2_demo.launch.py

Dans la deuxième fenêtre du terminal, tapez la commande suivante :

ros2 run turtlesim turtle_teleop_key

Vous verrez maintenant que la simulation turtlesim a commencé avec une tortue que vous pouvez contrôler.

Maintenant, utilisez l’outil tf2_echo pour vérifier si la pose de la tortue est réellement diffusée sur tf2 :

ros2 run tf2_ros tf2_echo world turtle1

Cela devrait vous montrer la pose de la première tortue. Conduisez autour de la tortue en utilisant les touches fléchées (assurez-vous que votre fenêtre de terminal turtle_teleop_key est active, pas la fenêtre de votre simulateur). Dans la sortie de votre console, vous verrez quelque chose de similaire à ceci :

At time 1625137663.912474878
- Translation: [5.276, 7.930, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137664.950813527
- Translation: [3.750, 6.563, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137665.906280726
- Translation: [2.320, 5.282, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.934, -0.357]
At time 1625137666.850775673
- Translation: [2.153, 5.133, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.365, 0.931]

Si vous exécutez tf2_echo pour la transformation entre world et turtle2, vous ne devriez pas voir de transformation, car la deuxième tortue n’est pas encore là. Cependant, dès que nous ajouterons la deuxième tortue dans le prochain tutoriel, la pose de turtle2 sera diffusée sur tf2.

Résumé

Dans ce tutoriel, vous avez appris à diffuser la pose du robot (position et orientation de la tortue) à tf2 et à utiliser l’outil tf2_echo. Pour utiliser réellement les transformations diffusées sur tf2, vous devez passer au didacticiel suivant sur la création d’un tf2 listener.