Ajouter un cadre (C++)

Objectif : Apprenez à ajouter une image supplémentaire à tf2.

Niveau du didacticiel : Intermédiaire

Durée : 15 minutes

Arrière-plan

Dans les tutoriels précédents, nous avons recréé la démo de la tortue en écrivant un tf2 broadcaster et un tf2 listener. Ce didacticiel vous apprendra comment ajouter des cadres fixes et dynamiques supplémentaires à l’arbre de transformation. En fait, l’ajout d’un cadre dans tf2 est très similaire à la création du diffuseur tf2, mais cet exemple vous montrera quelques fonctionnalités supplémentaires de tf2.

Pour de nombreuses tâches liées aux transformations, il est plus facile de penser à l’intérieur d’un cadre local. Par exemple, il est plus facile de raisonner sur les mesures de balayage laser dans un cadre au centre du scanner laser. tf2 vous permet de définir une trame locale pour chaque capteur, lien ou joint de votre système. Lors de la transformation d’une image à une autre, tf2 s’occupera de toutes les transformations d’images intermédiaires cachées qui sont introduites.

arbre tf2

tf2 construit une structure arborescente de trames et, par conséquent, ne permet pas une boucle fermée dans la structure de trame. Cela signifie qu’un cadre n’a qu’un seul parent, mais qu’il peut avoir plusieurs enfants. Actuellement, notre arborescence tf2 contient trois cadres : world, turtle1 et turtle2. Les deux cadres tortues sont des enfants du cadre monde. Si nous voulons ajouter un nouveau cadre à tf2, l’un des trois cadres existants doit être le cadre parent, et le nouveau deviendra son cadre enfant.

Tâches

1 Écrivez le diffuseur à cadre fixe

Dans notre exemple de tortue, nous allons ajouter un nouveau cadre carrot1, qui sera l’enfant de turtle1. Ce cadre servira de but pour la deuxième tortue.

Commençons par créer les fichiers source. Accédez au package learning_tf2_cpp que nous avons créé dans les tutoriels précédents. Téléchargez le code du diffuseur à cadre fixe en saisissant la commande suivante :

LinuxmacOSWindows

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

Ouvrez maintenant le fichier appelé fixed_frame_tf2_broadcaster.cpp.

#include <chrono>
#include <functional>
#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"

using namespace std::chrono_literals;

class FixedFrameBroadcaster : public rclcpp::Node
{
public:
  FixedFrameBroadcaster()
  : Node("fixed_frame_tf2_broadcaster")
  {
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    timer_ = this->create_wall_timer(
      100ms, std::bind(&FixedFrameBroadcaster::broadcast_timer_callback, this));
  }

private:
  void broadcast_timer_callback()
  {
    geometry_msgs::msg::TransformStamped t;

    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "turtle1";
    t.child_frame_id = "carrot1";
    t.transform.translation.x = 0.0;
    t.transform.translation.y = 2.0;
    t.transform.translation.z = 0.0;
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = 0.0;
    t.transform.rotation.w = 1.0;

    tf_broadcaster_->sendTransform(t);
  }

rclcpp::TimerBase::SharedPtr timer_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<FixedFrameBroadcaster>());
  rclcpp::shutdown();
  return 0;
}

Le code est très similaire à l’exemple du didacticiel du diffuseur tf2 et la seule différence est que la transformation ici ne change pas avec le temps.

1.1 Examiner le code

Examinons les lignes clés de ce morceau de code. Ici, nous créons une nouvelle transformation, du parent turtle1 au nouvel enfant carrot1. Le cadre carrot1 est décalé de 2 mètres sur l’axe y par rapport au cadre turtle1.

geometry_msgs::msg::TransformStamped t;

t.header.stamp = this->get_clock()->now();
t.header.frame_id = "turtle1";
t.child_frame_id = "carrot1";
t.transform.translation.x = 0.0;
t.transform.translation.y = 2.0;
t.transform.translation.z = 0.0;

1.2 CMakeLists.txt

Naviguez d’un niveau vers le répertoire learning_tf2_cpp, où se trouvent les fichiers CMakeLists.txt et package.xml.

Ouvrez maintenant le CMakeLists.txt ajoutez l’exécutable et nommez-le fixed_frame_tf2_broadcaster.

add_executable(fixed_frame_tf2_broadcaster src/fixed_frame_tf2_broadcaster.cpp)
ament_target_dependencies(
    fixed_frame_tf2_broadcaster
    geometry_msgs
    rclcpp
    tf2_ros
)

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

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

1.3 Écrire le fichier de lancement

Créons maintenant un fichier de lancement pour cet exemple. Avec votre éditeur de texte, créez un nouveau fichier appelé turtle_tf2_fixed_frame_demo.launch.py, et ajoutez les lignes suivantes :

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


def generate_launch_description():
    demo_nodes = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('learning_tf2_cpp'), 'launch'),
            '/turtle_tf2_demo.launch.py']),
        )

    return LaunchDescription([
        demo_nodes,
        Node(
            package='learning_tf2_cpp',
            executable='fixed_frame_tf2_broadcaster',
            name='fixed_broadcaster',
        ),
    ])

Ce fichier de lancement importe les packages requis, puis crée une variable demo_nodes qui stockera les nœuds que nous avons créés dans le fichier de lancement du didacticiel précédent.

La dernière partie du code ajoutera notre cadre fixe carrot1 au monde turtlesim en utilisant notre nœud fixed_frame_tf2_broadcaster.

Node(
    package='learning_tf2_cpp',
    executable='fixed_frame_tf2_broadcaster',
    name='fixed_broadcaster',
),

1.4 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

À partir de la racine de votre espace de travail, créez votre package mis à jour :

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

1.5 Exécuter

Vous pouvez maintenant démarrer la démo du diffuseur de tortue :

ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo.launch.py

Vous devriez remarquer que le nouveau cadre carrot1 est apparu dans l’arbre de transformation.

Si vous conduisez la première tortue, vous devriez remarquer que le comportement n’a pas changé par rapport au didacticiel précédent, même si nous avons ajouté une nouvelle image. En effet, l’ajout d’une image supplémentaire n’affecte pas les autres images et notre écouteur utilise toujours les images définies précédemment.

Par conséquent, si nous voulons que notre deuxième tortue suive la carotte au lieu de la première tortue, nous devons changer la valeur de target_frame. Cela peut être fait de deux façons. Une façon consiste à passer l’argument target_frame au fichier de lancement directement depuis la console :

ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo.launch.py target_frame:=carrot1

La deuxième méthode consiste à mettre à jour le fichier de lancement. Pour ce faire, ouvrez le fichier turtle_tf2_fixed_frame_demo.launch.py et ajoutez le paramètre 'target_frame': 'carrot1' via l’argument launch_arguments.

def generate_launch_description():
    demo_nodes = IncludeLaunchDescription(
        ...,
        launch_arguments={'target_frame': 'carrot1'}.items(),
        )

Maintenant, reconstruisez le paquet, redémarrez turtle_tf2_fixed_frame_demo.launch.py, et vous verrez la deuxième tortue suivre la carotte au lieu de la première tortue !

2 Écrire le diffuseur de trame dynamique

Le cadre supplémentaire que nous avons publié dans ce didacticiel est un cadre fixe qui ne change pas dans le temps par rapport au cadre parent. Cependant, si vous souhaitez publier une image mobile, vous pouvez coder le diffuseur pour modifier l’image au fil du temps. Modifions notre cadre carrot1 pour qu’il change par rapport au cadre turtle1 au fil du temps. Téléchargez maintenant le code du diffuseur de trames dynamiques en saisissant la commande suivante :

LinuxmacOSWindows

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

Ouvrez maintenant le fichier appelé dynamic_frame_tf2_broadcaster.cpp :

#include <chrono>
#include <functional>
#include <memory>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_broadcaster.h"

using namespace std::chrono_literals;

const double PI = 3.141592653589793238463;

class DynamicFrameBroadcaster : public rclcpp::Node
{
public:
  DynamicFrameBroadcaster()
  : Node("dynamic_frame_tf2_broadcaster")
  {
    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    timer_ = this->create_wall_timer(
      100ms, std::bind(&DynamicFrameBroadcaster::broadcast_timer_callback, this));
  }

private:
  void broadcast_timer_callback()
  {
    rclcpp::Time now = this->get_clock()->now();
    double x = now.seconds() * PI;

    geometry_msgs::msg::TransformStamped t;
    t.header.stamp = now;
    t.header.frame_id = "turtle1";
    t.child_frame_id = "carrot1";
    t.transform.translation.x = 10 * sin(x);
    t.transform.translation.y = 10 * cos(x);
    t.transform.translation.z = 0.0;
    t.transform.rotation.x = 0.0;
    t.transform.rotation.y = 0.0;
    t.transform.rotation.z = 0.0;
    t.transform.rotation.w = 1.0;

    tf_broadcaster_->sendTransform(t);
  }

  rclcpp::TimerBase::SharedPtr timer_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<DynamicFrameBroadcaster>());
  rclcpp::shutdown();
  return 0;
}

2.1 Examiner le code

Au lieu d’une définition fixe de nos décalages x et y, nous utilisons les fonctions sin() et cos() sur l’heure actuelle afin que le décalage de carrot1 change constamment.

double x = now.seconds() * PI;
...
t.transform.translation.x = 10 * sin(x);
t.transform.translation.y = 10 * cos(x);

2.2 CMakeLists.txt

Naviguez d’un niveau vers le répertoire learning_tf2_cpp, où se trouvent les fichiers CMakeLists.txt et package.xml.

Ouvrez maintenant le CMakeLists.txt ajoutez l’exécutable et nommez-le dynamic_frame_tf2_broadcaster.

add_executable(dynamic_frame_tf2_broadcaster src/dynamic_frame_tf2_broadcaster.cpp)
ament_target_dependencies(
    dynamic_frame_tf2_broadcaster
    geometry_msgs
    rclcpp
    tf2_ros
)

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

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

2.3 Écrire le fichier de lancement

Pour tester ce code, créez un nouveau fichier de lancement turtle_tf2_dynamic_frame_demo.launch.py et collez le code suivant :

import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource

from launch_ros.actions import Node


def generate_launch_description():
    demo_nodes = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([os.path.join(
            get_package_share_directory('learning_tf2_cpp'), 'launch'),
            '/turtle_tf2_demo.launch.py']),
        launch_arguments={'target_frame': 'carrot1'}.items(),
        )

    return LaunchDescription([
        demo_nodes,
        Node(
            package='learning_tf2_cpp',
            executable='dynamic_frame_tf2_broadcaster',
            name='dynamic_broadcaster',
        ),
    ])

2.4 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

À partir de la racine de votre espace de travail, créez votre package mis à jour :

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

2.5 Exécuter

Vous pouvez maintenant démarrer la démonstration du cadre dynamique :

ros2 launch learning_tf2_cpp turtle_tf2_dynamic_frame_demo.launch.py

Vous devriez voir que la deuxième tortue suit la position de la carotte qui change constamment.

Résumé

Dans ce didacticiel, vous avez découvert l’arbre de transformation tf2, sa structure et ses fonctionnalités. Vous avez également appris qu’il est plus facile de penser à l’intérieur d’un cadre local et appris à ajouter des cadres fixes et dynamiques supplémentaires pour ce cadre local.