Ecrire un diffuseur (C++)

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 didacticiel suppose que vous avez une connaissance pratique de ROS 2 et que vous avez terminé le didacticiel Introduction to tf2 tutorial et tf2 static broadcaster tutorial (C++). 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_cpp 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_cpp que nous avons créé dans le tutoriel précédent. Dans le répertoire src, téléchargez l’exemple de code de diffusion en saisissant la commande suivante :

LinuxmacOSWindows

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

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

#include <functional>
#include <memory>
#include <sstream>
#include <string>

#include "geometry_msgs/msg/transform_stamped.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/transform_broadcaster.h"
#include "turtlesim/msg/pose.hpp"

class FramePublisher : public rclcpp::Node
{
public:
  FramePublisher()
  : Node("turtle_tf2_frame_publisher")
  {
    // Declare and acquire `turtlename` parameter
    turtlename_ = this->declare_parameter<std::string>("turtlename", "turtle");

    // Initialize the transform broadcaster
    tf_broadcaster_ =
      std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    // Subscribe to a turtle{1}{2}/pose topic and call handle_turtle_pose
    // callback function on each message
    std::ostringstream stream;
    stream << "/" << turtlename_.c_str() << "/pose";
    std::string topic_name = stream.str();

    subscription_ = this->create_subscription<turtlesim::msg::Pose>(
      topic_name, 10,
      std::bind(&FramePublisher::handle_turtle_pose, this, std::placeholders::_1));
  }

private:
  void handle_turtle_pose(const std::shared_ptr<turtlesim::msg::Pose> msg)
  {
    geometry_msgs::msg::TransformStamped t;

    // Read message content and assign it to
    // corresponding tf variables
    t.header.stamp = this->get_clock()->now();
    t.header.frame_id = "world";
    t.child_frame_id = turtlename_.c_str();

    // 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
    tf2::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    t.transform.rotation.x = q.x();
    t.transform.rotation.y = q.y();
    t.transform.rotation.z = q.z();
    t.transform.rotation.w = q.w();

    // Send the transformation
    tf_broadcaster_->sendTransform(t);
  }

  rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subscription_;
  std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
  std::string turtlename_;
};

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

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.

turtlename_ = this->declare_parameter<std::string>("turtlename", "turtle");

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

subscription_ = this->create_subscription<turtlesim::msg::Pose>(
  topic_name, 10,
  std::bind(&FramePublisher::handle_turtle_pose, this, _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 this->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.

geometry_msgs::msg::TransformStamped t;

// Read message content and assign it to
// corresponding tf variables
t.header.stamp = this->get_clock()->now();
t.header.frame_id = "world";
t.child_frame_id = turtlename_.c_str();

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
tf2::Quaternion q;
q.setRPY(0, 0, msg->theta);
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 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
tf_broadcaster_->sendTransform(t);

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 turtle_tf2_broadcaster, que vous utiliserez plus tard avec ros2 run.

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

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

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

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_cpp',
            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_cpp',
    executable='turtle_tf2_broadcaster',
    name='broadcaster1',
    parameters=[
        {'turtlename': 'turtle1'}
    ]
),

2.2 Ajouter des dépendances

Naviguez d’un niveau vers le répertoire learning_tf2_cpp, où se trouvent les fichiers CMakeLists.txt 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 CMakeLists.txt

Rouvrez CMakeLists.txt et ajoutez la ligne pour que les fichiers de lancement du dossier launch/ soient installés.

install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME})

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

À 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

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_cpp 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 tutoriel suivant sur la création d’un tf2 listener.