Écrire un diffuseur statique (C++)

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_cpp dépendra de geometry_msgs, rclcpp, tf2, tf2_ros 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_cmake --dependencies geometry_msgs rclcpp tf2 tf2_ros turtlesim -- learning_tf2_cpp

Votre terminal renverra un message vérifiant la création de votre package learning_tf2_cpp 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_cpp/src, téléchargez l’exemple de code de diffusion statique en saisissant la commande suivante :

LinuxmacOSWindows

wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/static_turtle_tf2_broadcaster.cpp

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

#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/static_transform_broadcaster.h"

class StaticFramePublisher : public rclcpp::Node
{
public:
  explicit StaticFramePublisher(char * transformation[])
  : Node("static_turtle_tf2_broadcaster")
  {
    tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

    // Publish static transforms once at startup
    this->make_transforms(transformation);
  }

private:
  void make_transforms(char * transformation[])
  {
    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "world";
    t.child_frame_id = transformation[1];

    t.transform.translation.x = atof(transformation[2]);
    t.transform.translation.y = atof(transformation[3]);
    t.transform.translation.z = atof(transformation[4]);
    tf2::Quaternion q;
    q.setRPY(
      atof(transformation[5]),
      atof(transformation[6]),
      atof(transformation[7]));
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    tf_static_broadcaster_->sendTransform(t);
  }

  std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tf_static_broadcaster_;
};

int main(int argc, char * argv[])
{
  auto logger = rclcpp::get_logger("logger");

  // Obtain parameters from command line arguments
  if (argc != 8) {
    RCLCPP_INFO(
      logger, "Invalid number of parameters\nusage: "
      "$ ros2 run learning_tf2_cpp static_turtle_tf2_broadcaster "
      "child_frame_name x y z roll pitch yaw");
    return 1;
  }

  // As the parent frame of the transform is `world`, it is
  // necessary to check that the frame name passed is different
  if (strcmp(argv[1], "world") == 0) {
    RCLCPP_INFO(logger, "Your static turtle name cannot be 'world'");
    return 1;
  }

  // Pass parameters and initialize node
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<StaticFramePublisher>(argv));
  rclcpp::shutdown();
  return 0;
}

2.1 Examiner le code

Examinons maintenant le code pertinent pour publier la pose de tortue statique sur tf2. Les premières lignes incluent les fichiers d’en-tête requis. Nous incluons d’abord geometry_msgs/msg/transform_stamped.hpp pour accéder au type de message TransformStamped, que nous publierons dans l’arbre de transformation.

#include "geometry_msgs/msg/transform_stamped.hpp"

Ensuite, rclcpp est inclus afin que sa classe rclcpp::Node puisse être utilisée.

#include "rclcpp/rclcpp.hpp"

tf2::Quaternion est une classe pour un quaternion qui fournit des fonctions pratiques pour convertir les angles d’Euler en quaternions et vice versa. Nous incluons également tf2_ros/static_transform_broadcaster.h pour utiliser le StaticTransformBroadcaster afin de faciliter la publication des transformations statiques.

#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/static_transform_broadcaster.h"

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.

tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

this->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, this->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

geometry_msgs::msg::TransformStamped t;

t.header.stamp = this->get_clock()->now();
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 = atof(transformation[2]);
t.transform.translation.y = atof(transformation[3]);
t.transform.translation.z = atof(transformation[4]);
tf2::Quaternion q;
q.setRPY(
  atof(transformation[5]),
  atof(transformation[6]),
  atof(transformation[7]));
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();

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

tf_static_broadcaster_->sendTransform(t);

2.2 Ajouter des dépendances

Naviguez d’un niveau vers le répertoire src/learning_tf2_cpp, où les fichiers CMakeLists.txt 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 rclcpp</description>
<maintainer email="you@email.com">Your Name</maintainer>
<license>Apache License 2.0</license>

Assurez-vous de sauvegarder le fichier.

2.3 CMakeLists.txt

Ajoutez l’exécutable au CMakeLists.txt et nommez-le static_turtle_tf2_broadcaster, que vous utiliserez plus tard avec ros2 run.

add_executable(static_turtle_tf2_broadcaster src/static_turtle_tf2_broadcaster.cpp)
ament_target_dependencies(
   static_turtle_tf2_broadcaster
   geometry_msgs
   rclcpp
   tf2
   tf2_ros
)

Enfin, ajoutez la section install(TARGETS…) pour que ros2 run puisse trouver votre exécutable :

install(TARGETS
   static_turtle_tf2_broadcaster
   DESTINATION lib/${PROJECT_NAME})

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 :

LinuxmacOSWindows

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

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

LinuxmacOSWindows

colcon build --packages-select learning_tf2_cpp

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 nœud static_turtle_tf2_broadcaster :

ros2 run learning_tf2_cpp 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.