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
Contenu
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
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_message_broadcaster.py
Dans une invite de ligne de commande Windows :
curl -sk https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_message_broadcaster.py -o turtle_tf2_message_broadcaster.py
Ou en powershell :
curl https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_message_broadcaster.py -o 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
rosdep ne fonctionne que sous Linux, vous devrez donc installer vous-même les dépendances geometry_msgs
et turtlesim
rosdep ne fonctionne que sous Linux, vous devrez donc installer vous-même les dépendances geometry_msgs
et turtlesim
Et puis nous pouvons construire le package :
colcon build --packages-select learning_tf2_py
colcon build --packages-select learning_tf2_py
colcon build --merge-install --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
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/turtle_tf2_message_filter.cpp
Dans une invite de ligne de commande Windows :
curl -sk wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/turtle_tf2_message_filter.cpp -o turtle_tf2_message_filter.cpp
Ou en powershell :
curl https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/turtle_tf2_message_filter.cpp -o 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
rosdep ne fonctionne que sous Linux, vous devrez donc installer vous-même les dépendances geometry_msgs
et turtlesim
rosdep ne fonctionne que sous Linux, vous devrez donc installer vous-même les dépendances geometry_msgs
et turtlesim
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
colcon build --packages-select learning_tf2_cpp
colcon build --merge-install --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
. install/setup.bash
# CMD
call install\setup.bat
# Powershell
.\install\setup.ps1
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
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