Écrire un écouteur (Python)
Objectif : Apprendre à utiliser tf2 pour accéder aux transformations d’images.
Niveau du didacticiel : Intermédiaire
Durée : 10 minutes
Contenu
Arrière-plan
Dans les tutoriels précédents, nous avons créé un diffuseur tf2 pour publier la pose d’une tortue sur tf2.
Dans ce didacticiel, nous allons créer un écouteur tf2 pour commencer à utiliser tf2.
Conditions préalables
Ce didacticiel suppose que vous avez terminé le tutoriel du diffuseur tf2 (Python). Dans le tutoriel précédent, nous avons créé un package learning_tf2_py
, à partir duquel nous continuerons à travailler.
Tâches
1 Écrivez le nœud d’écoute
Commençons par créer les fichiers source. Accédez au package learning_tf2_py
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 d’écouteur en saisissant la commande suivante :
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.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_listener.py -o turtle_tf2_listener.py
Ou en powershell :
curl https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_py/turtle_tf2_py/turtle_tf2_listener.py -o turtle_tf2_listener.py
Ouvrez le fichier à l’aide de votre éditeur de texte préféré.
import math
from geometry_msgs.msg import Twist
import rclpy
from rclpy.node import Node
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from turtlesim.srv import Spawn
class FrameListener(Node):
def __init__(self):
super().__init__('turtle_tf2_frame_listener')
# Declare and acquire `target_frame` parameter
self.target_frame = self.declare_parameter(
'target_frame', 'turtle1').get_parameter_value().string_value
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
# 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
# Create turtle2 velocity publisher
self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1)
# Call on_timer function every second
self.timer = self.create_timer(1.0, self.on_timer)
def on_timer(self):
# Store frame names in variables that will be used to
# compute transformations
from_frame_rel = self.target_frame
to_frame_rel = 'turtle2'
if self.turtle_spawning_service_ready:
if self.turtle_spawned:
# Look up for the transformation between target_frame and turtle2 frames
# and send velocity commands for turtle2 to reach target_frame
try:
t = self.tf_buffer.lookup_transform(
to_frame_rel,
from_frame_rel,
rclpy.time.Time())
except TransformException as ex:
self.get_logger().info(
f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}')
return
msg = Twist()
scale_rotation_rate = 1.0
msg.angular.z = scale_rotation_rate * math.atan2(
t.transform.translation.y,
t.transform.translation.x)
scale_forward_speed = 0.5
msg.linear.x = scale_forward_speed * math.sqrt(
t.transform.translation.x ** 2 +
t.transform.translation.y ** 2)
self.publisher.publish(msg)
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 = 'turtle2'
request.x = float(4)
request.y = float(2)
request.theta = float(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')
def main():
rclpy.init()
node = FrameListener()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
rclpy.shutdown()
1.1 Examiner le code
Pour comprendre comment fonctionne le service derrière le frai de la tortue, veuillez vous référer à écrire un service simple et un client (Python) tutoriel.
Voyons maintenant le code pertinent pour accéder aux transformations de trame. Le paquet tf2_ros
fournit une implémentation d’un TransformListener
pour faciliter la tâche de réception des transformations.
from tf2_ros.transform_listener import TransformListener
Ici, nous créons un objet TransformListener
. Une fois l’écouteur créé, il commence à recevoir les transformations tf2 sur le réseau et les met en mémoire tampon pendant 10 secondes maximum.
self.tf_listener = TransformListener(self.tf_buffer, self)
Enfin, nous interrogeons l’écouteur pour une transformation spécifique. Nous appelons la méthode lookup_transform
avec les arguments suivants :
Cadre cible
Cadre source
Le moment où nous voulons transformer
Fournir rclpy.time.Time()
nous donnera simplement la dernière transformation disponible. Tout cela est enveloppé dans un bloc try-except pour gérer les éventuelles exceptions.
t = self.tf_buffer.lookup_transform(
to_frame_rel,
from_frame_rel,
rclpy.time.Time())
1.2 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
).
'turtle_tf2_listener = learning_tf2_py.turtle_tf2_listener:main',
2 Mettre à jour le fichier de lancement
Ouvrez le fichier de lancement appelé turtle_tf2_demo.launch.py
avec votre éditeur de texte, ajoutez deux nouveaux nœuds à la description de lancement, ajoutez un argument de lancement et ajoutez les importations. Le fichier résultant devrait ressembler à :
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='turtlesim',
executable='turtlesim_node',
name='sim'
),
Node(
package='learning_tf2_py',
executable='turtle_tf2_broadcaster',
name='broadcaster1',
parameters=[
{'turtlename': 'turtle1'}
]
),
DeclareLaunchArgument(
'target_frame', default_value='turtle1',
description='Target frame name.'
),
Node(
package='learning_tf2_py',
executable='turtle_tf2_broadcaster',
name='broadcaster2',
parameters=[
{'turtlename': 'turtle2'}
]
),
Node(
package='learning_tf2_py',
executable='turtle_tf2_listener',
name='listener',
parameters=[
{'target_frame': LaunchConfiguration('target_frame')}
]
),
])
Cela déclarera un argument de lancement target_frame
, démarrera un diffuseur pour la deuxième tortue que nous allons générer et un écouteur qui s’abonnera à ces transformations.
3 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
Toujours à la racine de votre espace de travail, construisez votre package :
colcon build --packages-select learning_tf2_py
colcon build --packages-select learning_tf2_py
colcon build --merge-install --packages-select learning_tf2_py
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
4 Courir
Vous êtes maintenant prêt à commencer votre démo complète de tortue :
ros2 launch learning_tf2_py turtle_tf2_demo.launch.py
Vous devriez voir la simulation de tortue avec deux tortues. Dans la deuxième fenêtre du terminal, tapez la commande suivante :
ros2 run turtlesim turtle_teleop_key
Pour voir si les choses fonctionnent, faites simplement le tour de la première tortue à l’aide des touches fléchées (assurez-vous que la fenêtre de votre terminal est active, pas la fenêtre de votre simulateur), et vous verrez la deuxième tortue suivre la première !
Résumé
Dans ce didacticiel, vous avez appris à utiliser tf2 pour accéder aux transformations de trame. Vous avez également fini d’écrire votre propre démo turtlesim que vous avez d’abord essayée dans Introduction to tf2 tutorial.