Ecriture d’un serveur d’action et d’un client (C++)
Objectif : Implémenter un serveur d’action et un client en C++.
Niveau du didacticiel : Intermédiaire
Durée : 15 minutes
Contenu
Arrière-plan
Les actions sont une forme de communication asynchrone dans ROS. Les clients d’action envoient des demandes d’objectif aux serveurs d’action. Les serveurs d’action envoient des commentaires sur les objectifs et les résultats aux clients d’action.
Conditions préalables
Vous aurez besoin du package action_tutorials_interfaces
et de l’interface Fibonacci.action
définie dans le tutoriel précédent, Créer une action.
Tâches
1 Création du package action_tutorials_cpp
Comme nous l’avons vu dans le tutoriel Création d’un paquet, nous devons créer un nouveau package pour contenir notre C++ et le code de support.
1.1 Création du package action_tutorials_cpp
Allez dans l’espace de travail d’action que vous avez créé dans le tutoriel précédent (n’oubliez pas de sourcer l’espace de travail), et créez un nouveau package pour le serveur d’action C++ :
cd ~/ros2_ws/src
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp
cd ~/ros2_ws/src
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp
cd \dev\ros2_ws\src
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp
1.2 Ajout du contrôle de la visibilité
Afin que le package soit compilé et fonctionne sous Windows, nous devons ajouter un « contrôle de visibilité ». Pour plus de détails, voir Visibilité des symboles Windows dans le document Trucs et astuces Windows.
Ouvrez action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.h
et insérez le code suivant :
#ifndef ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_
#define ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_
#ifdef __cplusplus
extern "C"
{
#endif
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((dllexport))
#define ACTION_TUTORIALS_CPP_IMPORT __attribute__ ((dllimport))
#else
#define ACTION_TUTORIALS_CPP_EXPORT __declspec(dllexport)
#define ACTION_TUTORIALS_CPP_IMPORT __declspec(dllimport)
#endif
#ifdef ACTION_TUTORIALS_CPP_BUILDING_DLL
#define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_EXPORT
#else
#define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_IMPORT
#endif
#define ACTION_TUTORIALS_CPP_PUBLIC_TYPE ACTION_TUTORIALS_CPP_PUBLIC
#define ACTION_TUTORIALS_CPP_LOCAL
#else
#define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((visibility("default")))
#define ACTION_TUTORIALS_CPP_IMPORT
#if __GNUC__ >= 4
#define ACTION_TUTORIALS_CPP_PUBLIC __attribute__ ((visibility("default")))
#define ACTION_TUTORIALS_CPP_LOCAL __attribute__ ((visibility("hidden")))
#else
#define ACTION_TUTORIALS_CPP_PUBLIC
#define ACTION_TUTORIALS_CPP_LOCAL
#endif
#define ACTION_TUTORIALS_CPP_PUBLIC_TYPE
#endif
#ifdef __cplusplus
}
#endif
#endif // ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_
2 Ecrire un serveur d’action
Concentrons-nous sur l’écriture d’un serveur d’action qui calcule la séquence de Fibonacci en utilisant l’action que nous avons créée dans le tutoriel Créer une action.
2.1 Ecriture du code du serveur d’action
Ouvrez action_tutorials_cpp/src/fibonacci_action_server.cpp
et insérez le code suivant :
#include <functional>
#include <memory>
#include <thread>
#include "action_tutorials_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"
#include "action_tutorials_cpp/visibility_control.h"
namespace action_tutorials_cpp
{
class FibonacciActionServer : public rclcpp::Node
{
public:
using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
ACTION_TUTORIALS_CPP_PUBLIC
explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("fibonacci_action_server", options)
{
using namespace std::placeholders;
this->action_server_ = rclcpp_action::create_server<Fibonacci>(
this,
"fibonacci",
std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
std::bind(&FibonacciActionServer::handle_cancel, this, _1),
std::bind(&FibonacciActionServer::handle_accepted, this, _1));
}
private:
rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const Fibonacci::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
(void)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse handle_cancel(
const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
using namespace std::placeholders;
// this needs to return quickly to avoid blocking the executor, so spin up a new thread
std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
}
void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing goal");
rclcpp::Rate loop_rate(1);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<Fibonacci::Feedback>();
auto & sequence = feedback->partial_sequence;
sequence.push_back(0);
sequence.push_back(1);
auto result = std::make_shared<Fibonacci::Result>();
for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
// Check if there is a cancel request
if (goal_handle->is_canceling()) {
result->sequence = sequence;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Goal canceled");
return;
}
// Update sequence
sequence.push_back(sequence[i] + sequence[i - 1]);
// Publish feedback
goal_handle->publish_feedback(feedback);
RCLCPP_INFO(this->get_logger(), "Publish feedback");
loop_rate.sleep();
}
// Check if goal is done
if (rclcpp::ok()) {
result->sequence = sequence;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
}
}
}; // class FibonacciActionServer
} // namespace action_tutorials_cpp
RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionServer)
Les premières lignes incluent tous les en-têtes que nous devons compiler.
Ensuite, nous créons une classe qui est une classe dérivée de rclcpp::Node
:
class FibonacciActionServer : public rclcpp::Node
Le constructeur de la classe FibonacciActionServer
initialise le nom du nœud en tant que fibonacci_action_server
:
explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("fibonacci_action_server", options)
Le constructeur instancie également un nouveau serveur d’action :
this->action_server_ = rclcpp_action::create_server<Fibonacci>(
this,
"fibonacci",
std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
std::bind(&FibonacciActionServer::handle_cancel, this, _1),
std::bind(&FibonacciActionServer::handle_accepted, this, _1));
Un serveur d’action nécessite 6 choses :
Le nom du type d’action basé sur un modèle :
Fibonacci
.Un nœud ROS 2 auquel ajouter l’action :
this
.Le nom de l’action :
'fibonacci'
.Une fonction de rappel pour gérer les objectifs :
handle_goal
Une fonction de rappel pour gérer l’annulation :
handle_cancel
.Une fonction de rappel pour gérer l’acceptation des objectifs :
handle_accept
.
La mise en œuvre des différents rappels est ensuite dans le fichier. Notez que tous les rappels doivent revenir rapidement, sinon nous risquons d’affamer l’exécuteur.
Nous commençons par le rappel pour gérer les nouveaux objectifs :
rclcpp_action::GoalResponse handle_goal(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const Fibonacci::Goal> goal)
{
RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
(void)uuid;
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
Cette implémentation accepte simplement tous les objectifs.
La prochaine étape est le rappel pour traiter l’annulation :
rclcpp_action::CancelResponse handle_cancel(
const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
(void)goal_handle;
return rclcpp_action::CancelResponse::ACCEPT;
}
Cette implémentation indique simplement au client qu’il a accepté l’annulation.
Le dernier des rappels accepte un nouvel objectif et commence à le traiter :
void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
using namespace std::placeholders;
// this needs to return quickly to avoid blocking the executor, so spin up a new thread
std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
}
Étant donné que l’exécution est une opération de longue durée, nous générons un thread pour effectuer le travail réel et revenons rapidement de handle_accepted
.
Tous les traitements et mises à jour ultérieurs sont effectués dans la méthode execute
du nouveau thread :
void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing goal");
rclcpp::Rate loop_rate(1);
const auto goal = goal_handle->get_goal();
auto feedback = std::make_shared<Fibonacci::Feedback>();
auto & sequence = feedback->partial_sequence;
sequence.push_back(0);
sequence.push_back(1);
auto result = std::make_shared<Fibonacci::Result>();
for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
// Check if there is a cancel request
if (goal_handle->is_canceling()) {
result->sequence = sequence;
goal_handle->canceled(result);
RCLCPP_INFO(this->get_logger(), "Goal canceled");
return;
}
// Update sequence
sequence.push_back(sequence[i] + sequence[i - 1]);
// Publish feedback
goal_handle->publish_feedback(feedback);
RCLCPP_INFO(this->get_logger(), "Publish feedback");
loop_rate.sleep();
}
// Check if goal is done
if (rclcpp::ok()) {
result->sequence = sequence;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Goal succeeded");
}
}
Ce fil de travail traite un numéro de séquence de la séquence de Fibonacci chaque seconde, publiant une mise à jour des commentaires pour chaque étape. Une fois le traitement terminé, il marque le goal_handle
comme réussi et quitte.
Nous avons maintenant un serveur d’action entièrement fonctionnel. Construisons-le et faisons-le fonctionner.
2.2 Compilation du serveur d’action
Dans la section précédente, nous avons mis en place le code du serveur d’action. Pour qu’il se compile et s’exécute, nous devons faire quelques choses supplémentaires.
Nous devons d’abord configurer CMakeLists.txt afin que le serveur d’action soit compilé. Ouvrez action_tutorials_cpp/CMakeLists.txt
et ajoutez ce qui suit juste après les appels find_package
:
add_library(action_server SHARED
src/fibonacci_action_server.cpp)
target_include_directories(action_server PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_definitions(action_server
PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL")
ament_target_dependencies(action_server
"action_tutorials_interfaces"
"rclcpp"
"rclcpp_action"
"rclcpp_components")
rclcpp_components_register_node(action_server PLUGIN "action_tutorials_cpp::FibonacciActionServer" EXECUTABLE fibonacci_action_server)
install(TARGETS
action_server
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)
Et maintenant nous pouvons compiler le package. Accédez au niveau supérieur de ros2_ws
et exécutez :
colcon build
Cela devrait compiler tout l’espace de travail, y compris le fibonacci_action_server
dans le paquet action_tutorials_cpp
.
2.3 Lancer le serveur d’action
Maintenant que nous avons construit le serveur d’action, nous pouvons l’exécuter. Sourcez l’espace de travail que nous venons de créer (ros2_ws
) et essayez d’exécuter le serveur d’action :
ros2 run action_tutorials_cpp fibonacci_action_server
3 Écrire un client d’action
3.1 Ecriture du code client de l’action
Ouvrez action_tutorials_cpp/src/fibonacci_action_client.cpp
et insérez le code suivant :
#include <functional>
#include <future>
#include <memory>
#include <string>
#include <sstream>
#include "action_tutorials_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"
namespace action_tutorials_cpp
{
class FibonacciActionClient : public rclcpp::Node
{
public:
using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;
explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
: Node("fibonacci_action_client", options)
{
this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
this,
"fibonacci");
this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&FibonacciActionClient::send_goal, this));
}
void send_goal()
{
using namespace std::placeholders;
this->timer_->cancel();
if (!this->client_ptr_->wait_for_action_server()) {
RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
rclcpp::shutdown();
}
auto goal_msg = Fibonacci::Goal();
goal_msg.order = 10;
RCLCPP_INFO(this->get_logger(), "Sending goal");
auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
send_goal_options.goal_response_callback =
std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
send_goal_options.feedback_callback =
std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2);
send_goal_options.result_callback =
std::bind(&FibonacciActionClient::result_callback, this, _1);
this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}
private:
rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
rclcpp::TimerBase::SharedPtr timer_;
void goal_response_callback(const GoalHandleFibonacci::SharedPtr & goal_handle)
{
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
} else {
RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
}
}
void feedback_callback(
GoalHandleFibonacci::SharedPtr,
const std::shared_ptr<const Fibonacci::Feedback> feedback)
{
std::stringstream ss;
ss << "Next number in sequence received: ";
for (auto number : feedback->partial_sequence) {
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
}
void result_callback(const GoalHandleFibonacci::WrappedResult & result)
{
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
return;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
return;
default:
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
return;
}
std::stringstream ss;
ss << "Result received: ";
for (auto number : result.result->sequence) {
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
rclcpp::shutdown();
}
}; // class FibonacciActionClient
} // namespace action_tutorials_cpp
RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionClient)
Les premières lignes incluent tous les en-têtes que nous devons compiler.
Ensuite, nous créons une classe qui est une classe dérivée de rclcpp::Node
:
class FibonacciActionClient : public rclcpp::Node
Le constructeur de la classe FibonacciActionClient
initialise le nom du nœud en tant que fibonacci_action_client
:
explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
: Node("fibonacci_action_client", options)
Le constructeur instancie également un nouveau client d’action :
this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
this,
"fibonacci");
Un client d’action nécessite 3 choses :
Le nom du type d’action basé sur un modèle :
Fibonacci
.Un nœud ROS 2 auquel ajouter le client d’action :
this
.Le nom de l’action :
'fibonacci'
.
Nous instancions également un minuteur ROS qui lancera le seul et unique appel à send_goal
:
this->timer_ = this->create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&FibonacciActionClient::send_goal, this));
Lorsque le minuteur expire, il appellera send_goal
:
void send_goal()
{
using namespace std::placeholders;
this->timer_->cancel();
if (!this->client_ptr_->wait_for_action_server()) {
RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
rclcpp::shutdown();
}
auto goal_msg = Fibonacci::Goal();
goal_msg.order = 10;
RCLCPP_INFO(this->get_logger(), "Sending goal");
auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
send_goal_options.goal_response_callback =
std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
send_goal_options.feedback_callback =
std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2);
send_goal_options.result_callback =
std::bind(&FibonacciActionClient::result_callback, this, _1);
this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}
Cette fonction effectue les opérations suivantes :
Annule la minuterie (elle n’est donc appelée qu’une seule fois).
Attend que le serveur d’action apparaisse.
Instancie un nouveau
Fibonacci::Goal
.Définit la réponse, les commentaires et les rappels de résultat.
Envoie l’objectif au serveur.
Lorsque le serveur reçoit et accepte l’objectif, il enverra une réponse au client. Cette réponse est gérée par goal_response_callback
:
void goal_response_callback(const GoalHandleFibonacci::SharedPtr & goal_handle)
{
if (!goal_handle) {
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
} else {
RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
}
}
En supposant que l’objectif a été accepté par le serveur, il commencera le traitement. Tout retour au client sera géré par le feedback_callback
:
void feedback_callback(
GoalHandleFibonacci::SharedPtr,
const std::shared_ptr<const Fibonacci::Feedback> feedback)
{
std::stringstream ss;
ss << "Next number in sequence received: ";
for (auto number : feedback->partial_sequence) {
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
}
Lorsque le serveur a terminé le traitement, il renverra un résultat au client. Le résultat est géré par le result_callback
:
void result_callback(const GoalHandleFibonacci::WrappedResult & result)
{
switch (result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
break;
case rclcpp_action::ResultCode::ABORTED:
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
return;
case rclcpp_action::ResultCode::CANCELED:
RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
return;
default:
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
return;
}
std::stringstream ss;
ss << "Result received: ";
for (auto number : result.result->sequence) {
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
rclcpp::shutdown();
}
}; // class FibonacciActionClient
Nous avons maintenant un client d’action entièrement fonctionnel. Construisons-le et faisons-le fonctionner.
3.2 Compilation du client d’action
Dans la section précédente, nous avons mis en place le code client d’action. Pour qu’il se compile et s’exécute, nous devons faire quelques choses supplémentaires.
Nous devons d’abord configurer CMakeLists.txt afin que le client d’action soit compilé. Ouvrez action_tutorials_cpp/CMakeLists.txt
et ajoutez ce qui suit juste après les appels find_package
:
add_library(action_client SHARED
src/fibonacci_action_client.cpp)
target_include_directories(action_client PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_definitions(action_client
PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL")
ament_target_dependencies(action_client
"action_tutorials_interfaces"
"rclcpp"
"rclcpp_action"
"rclcpp_components")
rclcpp_components_register_node(action_client PLUGIN "action_tutorials_cpp::FibonacciActionClient" EXECUTABLE fibonacci_action_client)
install(TARGETS
action_client
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)
Et maintenant nous pouvons compiler le package. Accédez au niveau supérieur de ros2_ws
et exécutez :
colcon build
Cela devrait compiler tout l’espace de travail, y compris le fibonacci_action_client
dans le paquet action_tutorials_cpp
.
3.3 Exécution du client d’action
Maintenant que nous avons construit le client d’action, nous pouvons l’exécuter. Assurez-vous d’abord qu’un serveur d’action est en cours d’exécution dans un terminal séparé. Sourcez maintenant l’espace de travail que nous venons de créer (ros2_ws
) et essayez d’exécuter le client d’action :
ros2 run action_tutorials_cpp fibonacci_action_client
Vous devriez voir les messages enregistrés pour l’objectif en cours d’acceptation, les commentaires en cours d’impression et le résultat final.