Utilisation du temps (C++)

Objectif : Apprendre à obtenir une transformation à un moment précis et attendre qu’une transformation soit disponible sur l’arborescence tf2 à l’aide de la fonction lookupTransform().

Niveau du didacticiel : Intermédiaire

Durée : 10 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 . Nous avons également appris à ajouter un nouveau cadre à l’arbre de transformation et appris comment tf2 garde la trace d’un arbre de cadres de coordonnées. Cette arborescence change au fil du temps et tf2 stocke un instantané de temps pour chaque transformation (jusqu’à 10 secondes par défaut). Jusqu’à présent, nous utilisions la fonction lookupTransform() pour accéder aux dernières transformations disponibles dans cet arbre tf2, sans savoir à quelle heure cette transformation a été enregistrée. Ce tutoriel vous apprendra comment obtenir une transformation à un moment précis.

Tâches

1 tf2 et le temps

Revenons donc là où nous nous sommes arrêtés dans le tutoriel sur l’ajout d’un cadre. Accédez au package learning_tf2_cpp. Ouvrez turtle_tf2_listener.cpp et regardez l’appel lookupTransform() :

transformStamped = tf_buffer_->lookupTransform(
   toFrameRel,
   fromFrameRel,
   tf2::TimePointZero);

Vous pouvez voir que nous avons spécifié un temps égal à 0 en appelant tf2::TimePointZero.

Note

Le paquet tf2 a son propre type de temps tf2::TimePoint, qui est différent de rclcpp::Time. De nombreuses API du package tf2_ros convertissent automatiquement entre rclcpp::Time et tf2::TimePoint.

rclcpp::Time(0, 0, this->get_clock()->get_clock_type()) aurait pu être utilisé ici, mais il aurait été converti en tf2::TimePointZero de toute façon.

Pour tf2, le temps 0 signifie « la dernière transformation disponible » dans le tampon. Maintenant, modifiez cette ligne pour obtenir la transformation à l’heure actuelle, this->get_clock()->now() :

rclcpp::Time now = this->get_clock()->now();
transformStamped = tf_buffer_->lookupTransform(
   toFrameRel,
   fromFrameRel,
   now);

Essayez maintenant d’exécuter le fichier de lancement.

ros2 launch learning_tf2_cpp turtle_tf2_demo.launch.py

Vous remarquerez qu’il échoue et produit quelque chose de similaire à ceci :

[INFO] [1629873136.345688064] [listener]: Could not transform turtle1 to turtle2: Lookup would
require extrapolation into the future.  Requested time 1629873136.345539 but the latest data
is at time 1629873136.338804, when looking up transform from frame [turtle1] to frame [turtle2]

Il vous indique que la trame n’existe pas ou que les données sont dans le futur.

Pour comprendre pourquoi cela se produit, nous devons comprendre comment fonctionnent les tampons. Tout d’abord, chaque écouteur dispose d’un buffer où il stocke toutes les transformées de coordonnées provenant des différents diffuseurs tf2. Deuxièmement, lorsqu’un diffuseur envoie une transformation, il faut un certain temps avant que cette transformation n’entre dans la mémoire tampon (généralement quelques millisecondes). Par conséquent, lorsque vous demandez une transformation de trame à l’heure « maintenant », vous devez attendre quelques millisecondes pour que ces informations arrivent.

2 Attendez les transformations

tf2 fournit un bel outil qui attendra qu’une transformation soit disponible. Vous l’utilisez en ajoutant un paramètre de délai d’attente à lookupTransform(). Pour résoudre ce problème, modifiez votre code comme indiqué ci-dessous (ajoutez le dernier paramètre de délai d’attente) :

rclcpp::Time now = this->get_clock()->now();
transformStamped = tf_buffer_->lookupTransform(
   toFrameRel,
   fromFrameRel,
   now,
   50ms);

Le lookupTransform() peut prendre quatre arguments, le dernier étant un délai d’attente facultatif. Il bloquera jusqu’à cette durée en attendant qu’il expire.

3 Vérification des résultats

Vous pouvez maintenant exécuter le fichier de lancement.

ros2 launch learning_tf2_cpp turtle_tf2_demo.launch.py

Vous devriez remarquer que lookupTransform() se bloquera jusqu’à ce que la transformation entre les deux tortues soit disponible (cela prendra généralement quelques millisecondes). Une fois le délai d’attente atteint (cinquante millisecondes dans ce cas), une exception sera levée uniquement si la transformation n’est toujours pas disponible.

Résumé

Dans ce didacticiel, vous avez appris à acquérir une transformation à un horodatage spécifique et à attendre qu’une transformation soit disponible sur l’arborescence tf2 lors de l’utilisation de la fonction lookupTransform().