Utiliser des types de données tamponnés avec tf2_ros::MessageFilter

Objectif : Apprendre à utiliser tf2_ros::MessageFilter pour traiter les types de données tamponnés.

Niveau du didacticiel : Intermédiaire

Durée : 10 minutes

Arrière-plan

Ce tutoriel explique comment utiliser les données du capteur avec tf2. Voici quelques exemples concrets de données de capteurs :

  • caméras, mono et stéréo

  • balayages laser

Supposons qu’une nouvelle tortue nommée turtle3 est créée et qu’elle n’a pas une bonne odométrie, mais qu’il y a une caméra aérienne qui suit sa position et la publie comme un message PointStamped en relation avec le ``monde` `cadre.

turtle1 veut savoir où turtle3 est comparé à lui-même.

Pour ce faire, turtle1 doit écouter le sujet où la pose de turtle3 est publiée, attendre que les transformations dans le cadre souhaité soient prêtes, puis effectuer ses opérations. Pour rendre cela plus facile, le tf2_ros::MessageFilter est très utile. Le tf2_ros::MessageFilter prendra un abonnement à n’importe quel message ROS 2 avec un en-tête et le mettra en cache jusqu’à ce qu’il soit possible de le transformer en trame cible.

Tâches

1 Écrire le nœud diffuseur des messages PointStamped

Pour ce tutoriel, nous allons configurer une application de démonstration qui a un nœud (en Python) pour diffuser les messages de position PointStamped de turtle3.

Commençons par créer le fichier source.

Accédez au learning_tf2_py package 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 de message de capteur en entrant la commande suivante :

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

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

from geometry_msgs.msg import PointStamped
from geometry_msgs.msg import Twist

import rclpy
from rclpy.node import Node

from turtlesim.msg import Pose
from turtlesim.srv import Spawn


class PointPublisher(Node):

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

        # Create a client to spawn a turtle
        self.spawner = self.create_client(Spawn, 'spawn')
        # Boolean values to store the information
        # if the service for spawning turtle is available
        self.turtle_spawning_service_ready = False
        # if the turtle was successfully spawned
        self.turtle_spawned = False
        # if the topics of turtle3 can be subscribed
        self.turtle_pose_cansubscribe = False

        self.timer = self.create_timer(1.0, self.on_timer)

    def on_timer(self):
        if self.turtle_spawning_service_ready:
            if self.turtle_spawned:
                self.turtle_pose_cansubscribe = True
            else:
                if self.result.done():
                    self.get_logger().info(
                        f'Successfully spawned {self.result.result().name}')
                    self.turtle_spawned = True
                else:
                    self.get_logger().info('Spawn is not finished')
        else:
            if self.spawner.service_is_ready():
                # Initialize request with turtle name and coordinates
                # Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
                request = Spawn.Request()
                request.name = 'turtle3'
                request.x = 4.0
                request.y = 2.0
                request.theta = 0.0
                # Call request
                self.result = self.spawner.call_async(request)
                self.turtle_spawning_service_ready = True
            else:
                # Check if the service is ready
                self.get_logger().info('Service is not ready')

        if self.turtle_pose_cansubscribe:
            self.vel_pub = self.create_publisher(Twist, 'turtle3/cmd_vel', 10)
            self.sub = self.create_subscription(Pose, 'turtle3/pose', self.handle_turtle_pose, 10)
            self.pub = self.create_publisher(PointStamped, 'turtle3/turtle_point_stamped', 10)

    def handle_turtle_pose(self, msg):
        vel_msg = Twist()
        vel_msg.linear.x = 1.0
        vel_msg.angular.z = 1.0
        self.vel_pub.publish(vel_msg)

        ps = PointStamped()
        ps.header.stamp = self.get_clock().now().to_msg()
        ps.header.frame_id = 'world'
        ps.point.x = msg.x
        ps.point.y = msg.y
        ps.point.z = 0.0
        self.pub.publish(ps)


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

    rclpy.shutdown()

1.1 Examiner le code

Voyons maintenant le code. Tout d’abord, dans la fonction de rappel on_timer, nous générons la turtle3 en appelant de manière asynchrone le service Spawn de turtlesim, et initialisons sa position à (4, 2, 0), lorsque le service de ponte des tortues est prêt.

# Initialize request with turtle name and coordinates
# Note that x, y and theta are defined as floats in turtlesim/srv/Spawn
request = Spawn.Request()
request.name = 'turtle3'
request.x = 4.0
request.y = 2.0
request.theta = 0.0
# Call request
self.result = self.spawner.call_async(request)

Ensuite, le nœud publie le sujet turtle3/cmd_vel, le sujet turtle3/turtle_point_stamped, et s’abonne au sujet turtle3/pose et exécute la fonction de rappel handle_turtle_pose sur chaque message entrant.

self.vel_pub = self.create_publisher(Twist, '/turtle3/cmd_vel', 10)
self.sub = self.create_subscription(Pose, '/turtle3/pose', self.handle_turtle_pose, 10)
self.pub = self.create_publisher(PointStamped, '/turtle3/turtle_point_stamped', 10)

Enfin, dans la fonction de rappel handle_turtle_pose, nous initialisons les messages Twist de turtle3 et les publions, ce qui fera déplacer la turtle3 le long d’un cercle. Ensuite, nous remplissons les messages PointStamped de turtle3 avec des messages Pose entrants et les publions.

vel_msg = Twist()
vel_msg.linear.x = 1.0
vel_msg.angular.z = 1.0
self.vel_pub.publish(vel_msg)

ps = PointStamped()
ps.header.stamp = self.get_clock().now().to_msg()
ps.header.frame_id = 'world'
ps.point.x = msg.x
ps.point.y = msg.y
ps.point.z = 0.0
self.pub.publish(ps)

1.2 Écrire le fichier de lancement

Afin d’exécuter cette démo, nous devons créer un fichier de lancement turtle_tf2_sensor_message.launch.py dans le sous-répertoire launch du package learning_tf2_py :

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        DeclareLaunchArgument(
            'target_frame', default_value='turtle1',
            description='Target frame name.'
        ),
        Node(
            package='turtlesim',
            executable='turtlesim_node',
            name='sim',
            output='screen'
        ),
        Node(
            package='turtle_tf2_py',
            executable='turtle_tf2_broadcaster',
            name='broadcaster1',
            parameters=[
                {'turtlename': 'turtle1'}
            ]
        ),
        Node(
            package='turtle_tf2_py',
            executable='turtle_tf2_broadcaster',
            name='broadcaster2',
            parameters=[
                {'turtlename': 'turtle3'}
            ]
        ),
        Node(
            package='turtle_tf2_py',
            executable='turtle_tf2_message_broadcaster',
            name='message_broadcaster',
        ),
    ])

1.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': :

'turtle_tf2_message_broadcaster = learning_tf2_py.turtle_tf2_message_broadcaster:main',

1.4 Construire

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

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

Et puis nous pouvons construire le package :

colcon build --packages-select learning_tf2_py

2 Ecriture du nœud filtre/écouteur de messages

Maintenant, pour obtenir les données de flux PointStamped de turtle3 dans le cadre de turtle1 de manière fiable, nous allons créer le fichier source du nœud filtre/écouteur de messages.

Accédez au learning_tf2_cpp package que nous avons créé dans le tutoriel précédent. Dans le répertoire src/learning_tf2_cpp/src, téléchargez le fichier turtle_tf2_message_filter.cpp en saisissant la commande suivante :

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

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

#include <chrono>
#include <memory>
#include <string>

#include "geometry_msgs/msg/point_stamped.hpp"
#include "message_filters/subscriber.h"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/create_timer_ros.h"
#include "tf2_ros/message_filter.h"
#include "tf2_ros/transform_listener.h"
#ifdef TF2_CPP_HEADERS
  #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#else
  #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#endif

using namespace std::chrono_literals;

class PoseDrawer : public rclcpp::Node
{
public:
  PoseDrawer()
  : Node("turtle_tf2_pose_drawer")
  {
    // Declare and acquire `target_frame` parameter
    target_frame_ = this->declare_parameter<std::string>("target_frame", "turtle1");

    std::chrono::duration<int> buffer_timeout(1);

    tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    // Create the timer interface before call to waitForTransform,
    // to avoid a tf2_ros::CreateTimerInterfaceException exception
    auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
      this->get_node_base_interface(),
      this->get_node_timers_interface());
    tf2_buffer_->setCreateTimerInterface(timer_interface);
    tf2_listener_ =
      std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);

    point_sub_.subscribe(this, "/turtle3/turtle_point_stamped");
    tf2_filter_ = std::make_shared<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>>(
      point_sub_, *tf2_buffer_, target_frame_, 100, this->get_node_logging_interface(),
      this->get_node_clock_interface(), buffer_timeout);
    // Register a callback with tf2_ros::MessageFilter to be called when transforms are available
    tf2_filter_->registerCallback(&PoseDrawer::msgCallback, this);
  }

private:
  void msgCallback(const geometry_msgs::msg::PointStamped::SharedPtr point_ptr)
  {
    geometry_msgs::msg::PointStamped point_out;
    try {
      tf2_buffer_->transform(*point_ptr, point_out, target_frame_);
      RCLCPP_INFO(
        this->get_logger(), "Point of turtle3 in frame of turtle1: x:%f y:%f z:%f\n",
        point_out.point.x,
        point_out.point.y,
        point_out.point.z);
    } catch (const tf2::TransformException & ex) {
      RCLCPP_WARN(
        // Print exception which was caught
        this->get_logger(), "Failure %s\n", ex.what());
    }
  }

  std::string target_frame_;
  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
  message_filters::Subscriber<geometry_msgs::msg::PointStamped> point_sub_;
  std::shared_ptr<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>> tf2_filter_;
};

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

2.1 Examiner le code

Tout d’abord, vous devez inclure les en-têtes tf2_ros::MessageFilter du paquet tf2_ros, ainsi que les en-têtes associés tf2 et ros2 précédemment utilisés.

#include "geometry_msgs/msg/point_stamped.hpp"
#include "message_filters/subscriber.h"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/create_timer_ros.h"
#include "tf2_ros/message_filter.h"
#include "tf2_ros/transform_listener.h"
#ifdef TF2_CPP_HEADERS
  #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#else
  #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#endif

Deuxièmement, il doit y avoir des instances persistantes de tf2_ros::Buffer, tf2_ros::TransformListener et tf2_ros::MessageFilter.

std::string target_frame_;
std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
message_filters::Subscriber<geometry_msgs::msg::PointStamped> point_sub_;
std::shared_ptr<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>> tf2_filter_;

Troisièmement, le message_filters::Subscriber de ROS 2 doit être initialisé avec le sujet. Et le tf2_ros::MessageFilter doit être initialisé avec cet objet Subscriber. Les autres arguments à noter dans le constructeur MessageFilter sont le target_frame et la fonction de rappel. Le cadre cible est le cadre dans lequel il s’assurera que canTransform réussira. Et la fonction de rappel est la fonction qui sera appelée lorsque les données seront prêtes.

PoseDrawer()
: Node("turtle_tf2_pose_drawer")
{
  // Declare and acquire `target_frame` parameter
  target_frame_ = this->declare_parameter<std::string>("target_frame", "turtle1");

  std::chrono::duration<int> buffer_timeout(1);

  tf2_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
  // Create the timer interface before call to waitForTransform,
  // to avoid a tf2_ros::CreateTimerInterfaceException exception
  auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
    this->get_node_base_interface(),
    this->get_node_timers_interface());
  tf2_buffer_->setCreateTimerInterface(timer_interface);
  tf2_listener_ =
    std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);

  point_sub_.subscribe(this, "/turtle3/turtle_point_stamped");
  tf2_filter_ = std::make_shared<tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped>>(
    point_sub_, *tf2_buffer_, target_frame_, 100, this->get_node_logging_interface(),
    this->get_node_clock_interface(), buffer_timeout);
  // Register a callback with tf2_ros::MessageFilter to be called when transforms are available
  tf2_filter_->registerCallback(&PoseDrawer::msgCallback, this);
}

Et enfin, la méthode de rappel appellera tf2_buffer_->transform lorsque les données seront prêtes et imprimeront la sortie sur la console.

private:
  void msgCallback(const geometry_msgs::msg::PointStamped::SharedPtr point_ptr)
  {
    geometry_msgs::msg::PointStamped point_out;
    try {
      tf2_buffer_->transform(*point_ptr, point_out, target_frame_);
      RCLCPP_INFO(
        this->get_logger(), "Point of turtle3 in frame of turtle1: x:%f y:%f z:%f\n",
        point_out.point.x,
        point_out.point.y,
        point_out.point.z);
    } catch (const tf2::TransformException & ex) {
      RCLCPP_WARN(
        // Print exception which was caught
        this->get_logger(), "Failure %s\n", ex.what());
    }
  }

2.2 Ajouter des dépendances

Avant de construire le package learning_tf2_cpp, veuillez ajouter deux autres dépendances dans le fichier package.xml de ce package :

<depend>message_filters</depend>
<depend>tf2_geometry_msgs</depend>

2.3 CMakeLists.txt

Et dans le fichier CMakeLists.txt, ajoutez deux lignes sous les dépendances existantes :

find_package(message_filters REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)

Les lignes ci-dessous traiteront des différences entre les distributions ROS :

if(TARGET tf2_geometry_msgs::tf2_geometry_msgs)
  get_target_property(_include_dirs tf2_geometry_msgs::tf2_geometry_msgs INTERFACE_INCLUDE_DIRECTORIES)
else()
  set(_include_dirs ${tf2_geometry_msgs_INCLUDE_DIRS})
endif()

find_file(TF2_CPP_HEADERS
  NAMES tf2_geometry_msgs.hpp
  PATHS ${_include_dirs}
  NO_CACHE
  PATH_SUFFIXES tf2_geometry_msgs
)

Après cela, ajoutez l’exécutable et nommez-le turtle_tf2_message_filter, que vous utiliserez plus tard avec ros2 run.

add_executable(turtle_tf2_message_filter src/turtle_tf2_message_filter.cpp)
ament_target_dependencies(
  turtle_tf2_message_filter
  geometry_msgs
  message_filters
  rclcpp
  tf2
  tf2_geometry_msgs
  tf2_ros
)

if(EXISTS ${TF2_CPP_HEADERS})
  target_compile_definitions(turtle_tf2_message_filter PUBLIC -DTF2_CPP_HEADERS)
endif()

Enfin, ajoutez la section install(TARGETS…) (sous les autres nœuds existants) afin que ros2 run puisse trouver votre exécutable :

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

2.4 Construire

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

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

Ouvrez maintenant un nouveau terminal, accédez à la racine de votre espace de travail et reconstruisez le package avec la commande :

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 :

. install/setup.bash

3 Courir

Nous devons d’abord exécuter plusieurs nœuds (dont le nœud diffuseur des messages PointStamped) en lançant le fichier de lancement turtle_tf2_sensor_message.launch.py :

ros2 launch learning_tf2_py turtle_tf2_sensor_message.launch.py

Cela fera apparaître la fenêtre turtlesim avec deux tortues, où turtle3 se déplace le long d’un cercle, tandis que turtle1 ne bouge pas au début. Mais vous pouvez exécuter le nœud turtle_teleop_key dans un autre terminal pour amener turtle1 à se déplacer :

ros2 run turtlesim turtle_teleop_key
../../../_images/turtlesim_messagefilter.png

Maintenant, si vous faites écho au sujet turtle3/turtle_point_stamped :

ros2 topic echo /turtle3/turtle_point_stamped

Ensuite, il y aura une sortie comme celle-ci :

header:
  stamp:
    sec: 1629877510
    nanosec: 902607040
  frame_id: world
point:
  x: 4.989276885986328
  y: 3.073937177658081
  z: 0.0
---
header:
  stamp:
    sec: 1629877510
    nanosec: 918389395
  frame_id: world
point:
  x: 4.987966060638428
  y: 3.089883327484131
  z: 0.0
---
header:
  stamp:
    sec: 1629877510
    nanosec: 934186680
  frame_id: world
point:
  x: 4.986400127410889
  y: 3.105806589126587
  z: 0.0
---

Lorsque la démo est en cours d’exécution, ouvrez un autre terminal et exécutez le nœud de filtre/écouteur de messages :

ros2 run learning_tf2_cpp turtle_tf2_message_filter

S’il fonctionne correctement, vous devriez voir des flux de données comme celui-ci :

[INFO] [1630016162.006173900] [turtle_tf2_pose_drawer]: Point of turtle3 in frame of turtle1: x:-6.493231 y:-2.961614 z:0.000000

[INFO] [1630016162.006291983] [turtle_tf2_pose_drawer]: Point of turtle3 in frame of turtle1: x:-6.472169 y:-3.004742 z:0.000000

[INFO] [1630016162.006326234] [turtle_tf2_pose_drawer]: Point of turtle3 in frame of turtle1: x:-6.479420 y:-2.990479 z:0.000000

[INFO] [1630016162.006355644] [turtle_tf2_pose_drawer]: Point of turtle3 in frame of turtle1: x:-6.486441 y:-2.976102 z:0.000000

Résumé

Dans ce didacticiel, vous avez appris à utiliser les données/messages des capteurs dans tf2. Plus précisément, vous avez appris à publier des messages PointStamped sur un sujet, et à écouter le sujet et à transformer le cadre des messages PointStamped avec tf2_ros::MessageFilter.