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

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

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 :

  1. Le nom du type d’action basé sur un modèle : Fibonacci.

  2. Un nœud ROS 2 auquel ajouter l’action : this.

  3. Le nom de l’action : 'fibonacci'.

  4. Une fonction de rappel pour gérer les objectifs : handle_goal

  5. Une fonction de rappel pour gérer l’annulation : handle_cancel.

  6. 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 :

  1. Le nom du type d’action basé sur un modèle : Fibonacci.

  2. Un nœud ROS 2 auquel ajouter le client d’action : this.

  3. 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 :

  1. Annule la minuterie (elle n’est donc appelée qu’une seule fois).

  2. Attend que le serveur d’action apparaisse.

  3. Instancie un nouveau Fibonacci::Goal.

  4. Définit la réponse, les commentaires et les rappels de résultat.

  5. 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.

Résumé

Dans ce didacticiel, vous avez assemblé ligne par ligne un serveur d’action C++ et un client d’action, et les avez configurés pour échanger des objectifs, des commentaires et des résultats.