Écrire un diffuseur statique (Python)

Objectif : Apprendre à diffuser des trames de coordonnées statiques vers tf2.

Niveau du didacticiel : Intermédiaire

Durée : 15 minutes

Arrière-plan

La publication de transformations statiques est utile pour définir la relation entre la base d’un robot et ses capteurs ou pièces fixes. Par exemple, il est plus facile de raisonner sur les mesures de balayage laser dans un cadre au centre du scanner laser.

Il s’agit d’un didacticiel autonome couvrant les bases des transformations statiques, qui se compose de deux parties. Dans la première partie, nous allons écrire du code pour publier des transformations statiques sur tf2. Dans la deuxième partie, nous expliquerons comment utiliser l’outil exécutable en ligne de commande static_transform_publisher dans tf2_ros.

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.

Conditions préalables

Dans les tutoriels précédents, vous avez appris à créer un espace de travail et créer un package.

Tâches

1 Créer un package

Nous allons d’abord créer un package qui sera utilisé pour ce tutoriel et les suivants. Le paquet appelé learning_tf2_py dépendra de geometry_msgs, python3-numpy, rclpy, tf2_ros_py et turtlesim. Le code de ce didacticiel est stocké ici.

Ouvrez un nouveau terminal et sourcez votre installation ROS 2 pour que les commandes ros2 fonctionnent. Accédez au dossier src de l’espace de travail et créez un nouveau package :

ros2 pkg create --build-type ament_python learning_tf2_py

Votre terminal renverra un message vérifiant la création de votre package learning_tf2_py et tous ses fichiers et dossiers nécessaires.

2 Écrivez le nœud diffuseur statique

Commençons par créer les fichiers source. Dans le répertoire src/learning_tf2_py/learning_tf2_py, téléchargez l’exemple de code de diffusion statique en saisissant la commande suivante :

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

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

import math
import sys

from geometry_msgs.msg import TransformStamped

import numpy as np

import rclpy
from rclpy.node import Node

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster


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 StaticFramePublisher(Node):
    """
    Broadcast transforms that never change.

    This example publishes transforms from `world` to a static turtle frame.
    The transforms are only published once at startup, and are constant for all
    time.
    """

    def __init__(self, transformation):
        super().__init__('static_turtle_tf2_broadcaster')

        self.tf_static_broadcaster = StaticTransformBroadcaster(self)

        # Publish static transforms once at startup
        self.make_transforms(transformation)

    def make_transforms(self, transformation):
        t = TransformStamped()

        t.header.stamp = self.get_clock().now().to_msg()
        t.header.frame_id = 'world'
        t.child_frame_id = transformation[1]

        t.transform.translation.x = float(transformation[2])
        t.transform.translation.y = float(transformation[3])
        t.transform.translation.z = float(transformation[4])
        quat = quaternion_from_euler(
            float(transformation[5]), float(transformation[6]), float(transformation[7]))
        t.transform.rotation.x = quat[0]
        t.transform.rotation.y = quat[1]
        t.transform.rotation.z = quat[2]
        t.transform.rotation.w = quat[3]

        self.tf_static_broadcaster.sendTransform(t)


def main():
    logger = rclpy.logging.get_logger('logger')

    # obtain parameters from command line arguments
    if len(sys.argv) != 8:
        logger.info('Invalid number of parameters. Usage: \n'
                    '$ ros2 run learning_tf2_py static_turtle_tf2_broadcaster'
                    'child_frame_name x y z roll pitch yaw')
        sys.exit(1)

    if sys.argv[1] == 'world':
        logger.info('Your static turtle name cannot be "world"')
        sys.exit(2)

    # pass parameters and initialize node
    rclpy.init()
    node = StaticFramePublisher(sys.argv)
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass

    rclpy.shutdown()

2.1 Examiner le code

Examinons maintenant le code pertinent pour publier la pose de tortue statique sur tf2. Les premières lignes importent les packages requis. Nous importons d’abord le TransformStamped du geometry_msgs, qui nous fournit un modèle pour le message que nous publierons dans l’arbre de transformation.

from geometry_msgs.msg import TransformStamped

Ensuite, rclpy est importé afin que sa classe Node puisse être utilisée.

import rclpy
from rclpy.node import Node

Le package tf2_ros fournit un StaticTransformBroadcaster pour faciliter la publication des transformations statiques. Pour utiliser le StaticTransformBroadcaster, nous devons l’importer depuis le module tf2_ros.

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

Le constructeur de classe StaticFramePublisher initialise le nœud avec le nom static_turtle_tf2_broadcaster. Ensuite, StaticTransformBroadcaster est créé, qui enverra une transformation statique au démarrage.

self.tf_static_broadcaster = StaticTransformBroadcaster(self)
self.make_transforms(transformation)

Ici, nous créons un objet TransformStamped, qui sera le message que nous enverrons une fois rempli. Avant de transmettre les valeurs de transformation réelles, nous devons lui donner 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, self.get_clock().now()

  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 cadre enfant du lien que nous créons

t = TransformStamped()

t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'world'
t.child_frame_id = transformation[1]

Ici, nous remplissons la pose 6D (translation et rotation) de la tortue.

t.transform.translation.x = float(transformation[2])
t.transform.translation.y = float(transformation[3])
t.transform.translation.z = float(transformation[4])
quat = quaternion_from_euler(
    float(transformation[5]), float(transformation[6]), float(transformation[7]))
t.transform.rotation.x = quat[0]
t.transform.rotation.y = quat[1]
t.transform.rotation.z = quat[2]
t.transform.rotation.w = quat[3]

Enfin, nous diffusons une transformation statique à l’aide de la fonction sendTransform().

self.tf_static_broadcaster.sendTransform(t)

2.2 Ajouter des dépendances

Naviguez d’un niveau vers le répertoire src/learning_tf2_py, où les fichiers setup.py, setup.cfg et package.xml ont été créés pour vous.

Ouvrez package.xml avec votre éditeur de texte.

Comme mentionné dans le tutoriel Create a package, assurez-vous de remplir le <description> , <responsable> et <licence> :

<description>Learning tf2 with rclpy</description>
<maintainer email="you@email.com">Your Name</maintainer>
<license>Apache License 2.0</license>

Après les lignes ci-dessus, ajoutez les dépendances suivantes correspondant aux instructions d’importation de votre nœud :

<exec_depend>geometry_msgs</exec_depend>
<exec_depend>python3-numpy</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>tf2_ros_py</exec_depend>
<exec_depend>turtlesim</exec_depend>

Cela déclare les dépendances requises geometry_msgs, python3-numpy, rclpy, tf2_ros_py et turtlesim lorsque son code est exécuté.

Assurez-vous de sauvegarder le fichier.

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

Ajoutez la ligne suivante entre les crochets 'console_scripts': :

'static_turtle_tf2_broadcaster = learning_tf2_py.static_turtle_tf2_broadcaster:main',

3 Construire

Il est recommandé d’exécuter rosdep à la racine de votre espace de travail pour vérifier les dépendances manquantes avant de compiler :

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

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

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 :

. install/setup.bash

4 Courir

Exécutez maintenant le nœud static_turtle_tf2_broadcaster :

ros2 run learning_tf2_py static_turtle_tf2_broadcaster mystaticturtle 0 0 1 0 0 0

Cela définit une pose de tortue diffusée pour `` mystaticturtle`` pour flotter à 1 mètre au-dessus du sol.

Nous pouvons maintenant vérifier que la transformation statique a été publiée en faisant écho au sujet tf_static

ros2 topic echo /tf_static

Si tout s’est bien passé, vous devriez voir une seule transformation statique

transforms:
- header:
   stamp:
      sec: 1622908754
      nanosec: 208515730
   frame_id: world
child_frame_id: mystaticturtle
transform:
   translation:
      x: 0.0
      y: 0.0
      z: 1.0
   rotation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0

La bonne façon de publier des transformations statiques

Ce tutoriel visait à montrer comment StaticTransformBroadcaster peut être utilisé pour publier des transformations statiques. Dans votre processus de développement réel, vous ne devriez pas avoir à écrire ce code vous-même et vous devriez utiliser l’outil dédié tf2_ros pour le faire. tf2_ros fournit un exécutable nommé static_transform_publisher qui peut être utilisé comme outil de ligne de commande ou comme nœud que vous pouvez ajouter à vos fichiers de lancement.

Publiez une transformation de coordonnées statiques sur tf2 en utilisant un décalage x/y/z en mètres et un roulis/tangage/lacet en radians. Dans notre cas, roulis/tangage/lacet fait référence à la rotation autour de l’axe x/y/z, respectivement.

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --yaw yaw --pitch pitch --roll roll --frame-id frame_id --child-frame-id child_frame_id

Publiez une transformation de coordonnées statiques sur tf2 en utilisant un décalage x/y/z en mètres et en quaternion.

ros2 run tf2_ros static_transform_publisher --x x --y y --z z --qx qx --qy qy --qz qz --qw qw --frame-id frame_id --child-frame-id child_frame_id

static_transform_publisher est conçu à la fois comme un outil de ligne de commande pour une utilisation manuelle, ainsi que pour une utilisation dans les fichiers launch pour définir des transformations statiques. Par exemple:

from launch import LaunchDescription
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            arguments = ['--x', '0', '--y', '0', '--z', '1', '--yaw', '0', '--pitch', '0', '--roll', '0', '--frame-id', 'world', '--child-frame-id', 'mystaticturtle']
        ),
    ])

Notez que tous les arguments sauf --frame-id et --child-frame-id sont facultatifs ; si une option particulière n’est pas spécifiée, l’identité sera supposée.

Résumé

Dans ce tutoriel, vous avez appris comment les transformations statiques sont utiles pour définir des relations statiques entre les cadres, comme mystaticturtle par rapport au cadre world. De plus, vous avez appris comment les transformations statiques peuvent être utiles pour comprendre les données des capteurs, comme celles des scanners laser, en reliant les données à un cadre de coordonnées commun. Enfin, vous avez écrit votre propre nœud pour publier des transformations statiques sur tf2 et appris à publier les transformations statiques requises à l’aide de l’exécutable static_transform_publisher et des fichiers de lancement.