Écrire un écouteur (C++)
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 tf2 static broadcaster tutorial (C++) et le tf2 broadcaster tutorial (C++). Dans le tutoriel précédent, nous avons créé un package learning_tf2_cpp
, à 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_cpp
que nous avons créé dans le tutoriel précédent. Dans le répertoire src
, 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_cpp/src/turtle_tf2_listener.cpp
wget https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/turtle_tf2_listener.cpp
Dans une invite de ligne de commande Windows :
curl -sk https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/turtle_tf2_listener.cpp -o turtle_tf2_listener.cpp
Ou en powershell :
curl https://raw.githubusercontent.com/ros/geometry_tutorials/ros2/turtle_tf2_cpp/src/turtle_tf2_listener.cpp -o turtle_tf2_listener.cpp
Ouvrez le fichier à l’aide de votre éditeur de texte préféré.
#include <chrono>
#include <functional>
#include <memory>
#include <string>
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/exceptions.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "turtlesim/srv/spawn.hpp"
using namespace std::chrono_literals;
class FrameListener : public rclcpp::Node
{
public:
FrameListener()
: Node("turtle_tf2_frame_listener"),
turtle_spawning_service_ready_(false),
turtle_spawned_(false)
{
// Declare and acquire `target_frame` parameter
target_frame_ = this->declare_parameter<std::string>("target_frame", "turtle1");
tf_buffer_ =
std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_listener_ =
std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
// Create a client to spawn a turtle
spawner_ =
this->create_client<turtlesim::srv::Spawn>("spawn");
// Create turtle2 velocity publisher
publisher_ =
this->create_publisher<geometry_msgs::msg::Twist>("turtle2/cmd_vel", 1);
// Call on_timer function every second
timer_ = this->create_wall_timer(
1s, std::bind(&FrameListener::on_timer, this));
}
private:
void on_timer()
{
// Store frame names in variables that will be used to
// compute transformations
std::string fromFrameRel = target_frame_.c_str();
std::string toFrameRel = "turtle2";
if (turtle_spawning_service_ready_) {
if (turtle_spawned_) {
geometry_msgs::msg::TransformStamped t;
// Look up for the transformation between target_frame and turtle2 frames
// and send velocity commands for turtle2 to reach target_frame
try {
t = tf_buffer_->lookupTransform(
toFrameRel, fromFrameRel,
tf2::TimePointZero);
} catch (const tf2::TransformException & ex) {
RCLCPP_INFO(
this->get_logger(), "Could not transform %s to %s: %s",
toFrameRel.c_str(), fromFrameRel.c_str(), ex.what());
return;
}
geometry_msgs::msg::Twist msg;
static const double scaleRotationRate = 1.0;
msg.angular.z = scaleRotationRate * atan2(
t.transform.translation.y,
t.transform.translation.x);
static const double scaleForwardSpeed = 0.5;
msg.linear.x = scaleForwardSpeed * sqrt(
pow(t.transform.translation.x, 2) +
pow(t.transform.translation.y, 2));
publisher_->publish(msg);
} else {
RCLCPP_INFO(this->get_logger(), "Successfully spawned");
turtle_spawned_ = true;
}
} else {
// Check if the service is ready
if (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
auto request = std::make_shared<turtlesim::srv::Spawn::Request>();
request->x = 4.0;
request->y = 2.0;
request->theta = 0.0;
request->name = "turtle2";
// Call request
using ServiceResponseFuture =
rclcpp::Client<turtlesim::srv::Spawn>::SharedFuture;
auto response_received_callback = [this](ServiceResponseFuture future) {
auto result = future.get();
if (strcmp(result->name.c_str(), "turtle2") == 0) {
turtle_spawning_service_ready_ = true;
} else {
RCLCPP_ERROR(this->get_logger(), "Service callback result mismatch");
}
};
auto result = spawner_->async_send_request(request, response_received_callback);
} else {
RCLCPP_INFO(this->get_logger(), "Service is not ready");
}
}
}
// Boolean values to store the information
// if the service for spawning turtle is available
bool turtle_spawning_service_ready_;
// if the turtle was successfully spawned
bool turtle_spawned_;
rclcpp::Client<turtlesim::srv::Spawn>::SharedPtr spawner_{nullptr};
rclcpp::TimerBase::SharedPtr timer_{nullptr};
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_{nullptr};
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
std::string target_frame_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<FrameListener>());
rclcpp::shutdown();
return 0;
}
1.1 Examiner le code
Pour comprendre comment fonctionne le service derrière la reproduction de la tortue, veuillez vous référer à écrire un service simple et un client (C++) tutoriel.
Voyons maintenant le code pertinent pour accéder aux transformations de trame. Le tf2_ros
contient une implémentation de fichier d’en-tête TransformListener
qui facilite la tâche de réception des transformations.
#include "tf2_ros/transform_listener.h"
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.
tf_listener_ =
std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
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 tf2::TimePointZero()
nous donnera simplement la dernière transformation disponible. Tout cela est enveloppé dans un bloc try-catch pour gérer les éventuelles exceptions.
t = tf_buffer_->lookupTransform(
toFrameRel, fromFrameRel,
tf2::TimePointZero);
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_listener
, que vous utiliserez plus tard avec ros2 run
.
add_executable(turtle_tf2_listener src/turtle_tf2_listener.cpp)
ament_target_dependencies(
turtle_tf2_listener
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_listener
DESTINATION lib/${PROJECT_NAME})
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_cpp',
executable='turtle_tf2_broadcaster',
name='broadcaster1',
parameters=[
{'turtlename': 'turtle1'}
]
),
DeclareLaunchArgument(
'target_frame', default_value='turtle1',
description='Target frame name.'
),
Node(
package='learning_tf2_cpp',
executable='turtle_tf2_broadcaster',
name='broadcaster2',
parameters=[
{'turtlename': 'turtle2'}
]
),
Node(
package='learning_tf2_cpp',
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
À partir de la racine de votre espace de travail, créez votre package mis à jour :
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
4 Courir
Vous êtes maintenant prêt à commencer votre démo complète de tortue :
ros2 launch learning_tf2_cpp 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.