【ROS2】中级-编写动作服务器和客户端（C++）

十年一梦实验室2024-07-10 18:27

目标：用 C++实现一个动作服务器和客户端。

教程级别：中级

时间：15 分钟

目录

  • 背景

  • 先决条件

  • 任务

      1. 创建 custom_action_cpp 包
      1. 编写动作服务器
      1. 编写动作客户端

  • 摘要

  • 相关内容

背景

动作是 ROS 中异步通信的一种形式 。动作客户端向动作服务器发送目标请求 。动作服务器向动作客户端发送目标反馈和结果。

先决条件

创建一个动作，您将需要 custom_action_interfaces 包和在上一教程中定义的 Fibonacci.action 接口。

任务

1. 创建 custom_action_cpp 包

在我们在创建包教程中看到的，我们需要创建一个新的包来容纳我们的 C++和支持代码。

1.1 创建 custom_action_cpp 包

进入您在上一个教程中创建的动作工作区（记得要引用工作区），并为 C++ 动作服务器创建一个新包：

properties 复制代码 
cd ~/ros2_ws/src
cxy@ubuntu2404-cxy:~/ros2_ws/src$ ros2 pkg create --dependencies custom_action_interfaces rclcpp rclcpp_action rclcpp_components --license Apache-2.0 -- custom_action_cpp
going to create a new package
package name: custom_action_cpp
destination directory: /home/cxy/ros2_ws/src
package format: 3
version: 0.0.0
description: TODO: Package description
maintainer: ['cxy <cxy@todo.todo>']
licenses: ['Apache-2.0']
build type: ament_cmake
dependencies: ['custom_action_interfaces', 'rclcpp', 'rclcpp_action', 'rclcpp_components']
creating folder ./custom_action_cpp
creating ./custom_action_cpp/package.xml
creating source and include folder
creating folder ./custom_action_cpp/src
creating folder ./custom_action_cpp/include/custom_action_cpp
creating ./custom_action_cpp/CMakeLists.txt
1.2 添加可见性控制

为了使软件包在 Windows 上编译和运行，我们需要添加一些"可见性控制"。有关更多详情，请参阅 Windows 提示和技巧文档中的 Windows 符号可见性 https://docs.ros.org/en/jazzy/The-ROS2-Project/Contributing/Windows-Tips-and-Tricks.html#windows-symbol-visibility 。

打开 custom_action_cpp/include/custom_action_cpp/visibility_control.h ，然后输入以下代码：

ruby 复制代码 
cxy@ubuntu2404-cxy:~/ros2_ws/src/custom_action_cpp$ gedit include/custom_action_cpp/visibility_control.h
cpp 复制代码 
#ifndef CUSTOM_ACTION_CPP__VISIBILITY_CONTROL_H_  // 如果没有定义 CUSTOM_ACTION_CPP__VISIBILITY_CONTROL_H_
#define CUSTOM_ACTION_CPP__VISIBILITY_CONTROL_H_  // 定义 CUSTOM_ACTION_CPP__VISIBILITY_CONTROL_H_


#ifdef __cplusplus  // 如果使用 C++ 编译器
extern "C"  // 使用 C 语言的链接方式
{
#endif


// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
// 这段逻辑借鉴并命名空间化自 gcc wiki 上的示例:
//     https://gcc.gnu.org/wiki/Visibility


#if defined _WIN32 || defined __CYGWIN__  // 如果定义了 _WIN32 或 __CYGWIN__
  #ifdef __GNUC__  // 如果使用 GNU 编译器
    #define CUSTOM_ACTION_CPP_EXPORT __attribute__ ((dllexport))  // 定义导出宏
    #define CUSTOM_ACTION_CPP_IMPORT __attribute__ ((dllimport))  // 定义导入宏
  #else
    #define CUSTOM_ACTION_CPP_EXPORT __declspec(dllexport)  // 定义导出宏
    #define CUSTOM_ACTION_CPP_IMPORT __declspec(dllimport)  // 定义导入宏
  #endif
  #ifdef CUSTOM_ACTION_CPP_BUILDING_DLL  // 如果正在构建 DLL
    #define CUSTOM_ACTION_CPP_PUBLIC CUSTOM_ACTION_CPP_EXPORT  // 定义公共宏为导出宏
  #else
    #define CUSTOM_ACTION_CPP_PUBLIC CUSTOM_ACTION_CPP_IMPORT  // 定义公共宏为导入宏
  #endif
  #define CUSTOM_ACTION_CPP_PUBLIC_TYPE CUSTOM_ACTION_CPP_PUBLIC  // 定义公共类型宏
  #define CUSTOM_ACTION_CPP_LOCAL  // 定义本地宏为空
#else
  #define CUSTOM_ACTION_CPP_EXPORT __attribute__ ((visibility("default")))  // 定义导出宏
  #define CUSTOM_ACTION_CPP_IMPORT  // 定义导入宏为空
  #if __GNUC__ >= 4  // 如果 GNU 编译器版本大于等于 4
    #define CUSTOM_ACTION_CPP_PUBLIC __attribute__ ((visibility("default")))  // 定义公共宏
    #define CUSTOM_ACTION_CPP_LOCAL  __attribute__ ((visibility("hidden")))  // 定义本地宏
  #else
    #define CUSTOM_ACTION_CPP_PUBLIC  // 定义公共宏为空
    #define CUSTOM_ACTION_CPP_LOCAL  // 定义本地宏为空
  #endif
  #define CUSTOM_ACTION_CPP_PUBLIC_TYPE  // 定义公共类型宏为空
#endif


#ifdef __cplusplus  // 如果使用 C++ 编译器
}
#endif


#endif  // CUSTOM_ACTION_CPP__VISIBILITY_CONTROL_H_  // 结束预处理指令

2. 编写动作服务器

让我们专注于编写一个动作服务器，使用我们在创建动作教程中创建的动作https://docs.ros.org/en/jazzy/Tutorials/Intermediate/Creating-an-Action.html 来计算斐波那契序列。

2.1 编写动作服务器代码

打开 custom_action_cpp/src/fibonacci_action_server.cpp ，然后输入以下代码：

php 复制代码 
#include <functional>  // 包含功能库
#include <memory>  // 包含内存管理库
#include <thread>  // 包含线程库


#include "custom_action_interfaces/action/fibonacci.hpp"  // 包含自定义动作接口的斐波那契头文件
#include "rclcpp/rclcpp.hpp"  // 包含ROS 2的C++客户端库
#include "rclcpp_action/rclcpp_action.hpp"  // 包含ROS 2的动作库
#include "rclcpp_components/register_node_macro.hpp"  // 包含ROS 2的节点注册宏


#include "custom_action_cpp/visibility_control.h"  // 包含自定义动作的可见性控制头文件


namespace custom_action_cpp  // 定义命名空间 custom_action_cpp
{
class FibonacciActionServer : public rclcpp::Node  // 定义 FibonacciActionServer 类，继承自 rclcpp::Node
{
public:
  using Fibonacci = custom_action_interfaces::action::Fibonacci;  // 定义 Fibonacci 类型
  using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;  // 定义 GoalHandleFibonacci 类型


  CUSTOM_ACTION_CPP_PUBLIC  // 定义公共可见性
  explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())  // 显式构造函数
  : Node("fibonacci_action_server", options)  // 调用基类构造函数，初始化节点名称为 "fibonacci_action_server"
  {
    using namespace std::placeholders;  // 使用占位符命名空间


    auto handle_goal = [this](
      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;  // 忽略 uuid
      return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;  // 接受并执行目标
    };


    auto handle_cancel = [this](
      const std::shared_ptr<GoalHandleFibonacci> goal_handle)
    {
      RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");  // 打印接收到的取消请求
      (void)goal_handle;  // 忽略 goal_handle
      return rclcpp_action::CancelResponse::ACCEPT;  // 接受取消请求
    };


    auto handle_accepted = [this](
      const std::shared_ptr<GoalHandleFibonacci> goal_handle)
    {
      // this needs to return quickly to avoid blocking the executor,
      // 需要快速返回以避免阻塞执行器，
      // so we declare a lambda function to be called inside a new thread
      // 因此我们声明一个 lambda 函数在新线程中调用
      auto execute_in_thread = this, goal_handle{return this->execute(goal_handle);};  // 定义在新线程中执行的 lambda 函数
      std::thread{execute_in_thread}.detach();  // 创建并分离线程
    };


    this->action_server_ = rclcpp_action::create_server<Fibonacci>(  // 创建动作服务器
      this,  // 当前节点
      "fibonacci",  // 动作名称
      handle_goal,  // 处理目标请求的函数
      handle_cancel,  // 处理取消请求的函数
      handle_accepted);  // 处理接受请求的函数
  }


private:
  rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;  // 动作服务器的共享指针


  void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle) {  // 执行目标的函数
    RCLCPP_INFO(this->get_logger(), "Executing goal");  // 打印正在执行目标
    rclcpp::Rate loop_rate(1);  // 设置循环频率为1Hz
    const auto goal = goal_handle->get_goal();  // 获取目标
    auto feedback = std::make_shared<Fibonacci::Feedback>();  // 创建反馈的共享指针
    auto & sequence = feedback->partial_sequence;  // 获取部分序列的引用
    sequence.push_back(0);  // 添加初始值0
    sequence.push_back(1);  // 添加初始值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()) {  // 如果 ROS 2 正常运行
      result->sequence = sequence;  // 设置结果序列
      goal_handle->succeed(result);  // 目标成功
      RCLCPP_INFO(this->get_logger(), "Goal succeeded");  // 打印目标成功
    }
  };


};  // class FibonacciActionServer


}  // namespace custom_action_cpp


RCLCPP_COMPONENTS_REGISTER_NODE(custom_action_cpp::FibonacciActionServer)  // 注册 FibonacciActionServer 节点

前几行包括了我们编译所需的所有头文件。

接下来我们创建一个类，这个类是 rclcpp::Node 的派生类：

cpp 复制代码 
class FibonacciActionServer : public rclcpp::Node

FibonacciActionServer 类的构造函数将节点名称初始化为 fibonacci_action_server

php 复制代码 
explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node("fibonacci_action_server", options)

构造函数还实例化了一个新的动作服务器：

kotlin 复制代码 
this->action_server_ = rclcpp_action::create_server<Fibonacci>(  // 创建动作服务器
      this,  // 当前节点
      "fibonacci",  // 动作名称
      handle_goal,  // 处理目标请求的函数
      handle_cancel,  // 处理取消请求的函数
      handle_accepted);  // 处理接受请求的函数

动作服务器需要 6 件事：

  1. 模板化的动作类型名称： Fibonacci 。

  2. 一个 ROS 2 节点用于添加动作： this 。

  3. 动作名称： 'fibonacci' 。

  4. 处理目标的回调函数： handle_goal

  5. 处理取消的回调函数： handle_cancel 。

  6. 处理目标接受的回调函数： handle_accept 。

各种回调的实现是在构造函数中用 lambda 表达式完成的。请注意，所有回调都需要快速返回，否则我们冒着使执行器饥饿的风险。

我们从处理新目标的回调开始：

php 复制代码 
auto handle_goal = [this](
      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;
    };

这个实现只接受所有目标。

接下来是处理取消的回调：

cpp 复制代码 
auto handle_cancel = [this](
      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;
    };

这个实现只是告诉客户它接受了取消。

最后一个回调接受一个新目标并开始处理它：

cpp 复制代码 
auto handle_accepted = [this](
      const std::shared_ptr<GoalHandleFibonacci> goal_handle)
    {
      // this needs to return quickly to avoid blocking the executor,
      // so we declare a lambda function to be called inside a new thread
      auto execute_in_thread = [this, goal_handle](){return this->execute(goal_handle);};
      std::thread{execute_in_thread}.detach();
    };

由于执行是一个长时间运行的操作，我们会分出一个线程来进行实际工作，并且从 handle_accepted 迅速返回。

所有进一步的处理和更新都在新线程中的 execute 方法中完成：

php 复制代码 
// 定义执行函数，参数为Fibonacci目标句柄的共享指针
void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle) {
    // 输出信息日志，打印"Executing goal"
    RCLCPP_INFO(this->get_logger(), "执行目标");
    // 创建循环速率对象，频率为1Hz
    rclcpp::Rate loop_rate(1);
    // 获取目标
    const auto goal = goal_handle->get_goal();
    // 创建Fibonacci反馈的共享指针
    auto feedback = std::make_shared<Fibonacci::Feedback>();
    // 获取反馈的部分序列
    auto & sequence = feedback->partial_sequence;
    // 将0和1添加到序列中
    sequence.push_back(0);
    sequence.push_back(1);
    // 创建Fibonacci结果的共享指针
    auto result = std::make_shared<Fibonacci::Result>();


    // 循环，直到达到目标顺序或rclcpp不再ok
    for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
      // 检查是否有取消请求
      if (goal_handle->is_canceling()) {
        // 如果有，将序列设置为结果，取消目标，并输出信息日志，打印"Goal canceled"
        result->sequence = sequence;
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "目标被取消");
        return;
      }
      // 更新序列
      sequence.push_back(sequence[i] + sequence[i - 1]);
      // 发布反馈
      goal_handle->publish_feedback(feedback);
      // 输出信息日志，打印"Publish feedback"
      RCLCPP_INFO(this->get_logger(), "发布反馈");


      // 按照循环速率休眠
      loop_rate.sleep();
    }


    // 检查目标是否完成
    if (rclcpp::ok()) {
      // 如果完成，将序列设置为结果，成功完成目标，并输出信息日志，打印"Goal succeeded"
      result->sequence = sequence;
      goal_handle->succeed(result);
      RCLCPP_INFO(this->get_logger(), "目标成功");
    }
  };

获取目标、创建反馈指针、创建结果指针。循环遍历斐波那契数列{ （如果目标正在取消，设置结果序列，取消目标） 计算下一个斐波那契数，发布反馈。}如果ROS2正常运行，设置结果序列，发送成功结果

这个工作线程每秒处理一个斐波那契序列的序号，并为每一步发布一个反馈更新。当它处理完成后，它将 goal_handle 标记为成功，并退出。

我们现在有了一个功能齐全的动作服务器。让我们构建并运行它。

2.2 编译动作服务器

在上一节中，我们已经将动作服务器代码放到了适当的位置。为了使其编译并运行，我们还需要做一些额外的工作。

首先我们需要设置 CMakeLists.txt，以便编译动作服务器。打开 custom_action_cpp/CMakeLists.txt ，并在 find_package 调用之后立即添加以下内容：

php 复制代码 
// 添加一个名为action_server的共享库，源文件为src/fibonacci_action_server.cpp
add_library(action_server SHARED
  src/fibonacci_action_server.cpp)


// 为action_server目标添加私有包含目录，这些目录在构建和安装阶段有所不同
target_include_directories(action_server PRIVATE
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>  // 构建阶段的包含目录
  $<INSTALL_INTERFACE:include>)  // 安装阶段的包含目录


// 为action_server目标添加私有编译定义
target_compile_definitions(action_server
  PRIVATE "CUSTOM_ACTION_CPP_BUILDING_DLL")


// 为action_server目标添加ament依赖项
ament_target_dependencies(action_server
  "custom_action_interfaces"  // 自定义动作接口
  "rclcpp"  // ROS客户端库
  "rclcpp_action"  // ROS动作客户端库
  "rclcpp_components")  // ROS组件库


// 注册一个名为action_server的节点插件，插件名为custom_action_cpp::FibonacciActionServer，可执行文件名为fibonacci_action_server
rclcpp_components_register_node(action_server PLUGIN "custom_action_cpp::FibonacciActionServer" EXECUTABLE fibonacci_action_server)


// 安装目标文件
install(TARGETS
  action_server  // 要安装的目标
  ARCHIVE DESTINATION lib  // 静态库的目标目录
  LIBRARY DESTINATION lib  // 动态库的目标目录
  RUNTIME DESTINATION bin)  // 可执行文件的目标目录

现在我们可以编译包了。转到 ros2_ws 的顶层，然后运行：

nginx 复制代码 
colcon build

这应该编译整个工作空间，包括 custom_action_cpp 包中的 fibonacci_action_server 。

go 复制代码 
cxy@ubuntu2404-cxy:~/ros2_ws$ colcon build --packages-select custom_action_cpp
Starting >>> custom_action_cpp
Finished <<< custom_action_cpp [14.9s]                       


Summary: 1 package finished [15.1s]
2.3 运行动作服务器

现在我们已经构建了动作服务器，我们可以运行它。源自我们刚刚构建的工作空间（ ros2_ws ），并尝试运行动作服务器：

nginx 复制代码 
ros2 run custom_action_cpp fibonacci_action_server

3. 编写动作客户端

3.1 编写动作客户端代码

打开 custom_action_cpp/src/fibonacci_action_client.cpp ，然后输入以下代码：ROS2动作客户端的实现，它创建了一个名为FibonacciActionClient的类，用于向动作服务器发送Fibonacci序列计算的目标，并处理服务器的响应和反馈。

php 复制代码 
// 包含C++标准库的功能头文件
#include <functional>
// 包含C++标准库的未来头文件，用于异步操作
#include <future>
// 包含C++标准库的内存头文件
#include <memory>
// 包含C++标准库的字符串头文件
#include <string>
// 包含C++标准库的字符串流头文件
#include <sstream>


// 包含自定义的Fibonacci动作接口
#include "custom_action_interfaces/action/fibonacci.hpp"


// 包含ROS2的节点类头文件
#include <rclcpp/rclcpp.hpp>
// 包含ROS2的动作客户端类头文件
#include <rclcpp_action/rclcpp_action.hpp>
// 包含ROS2的节点注册宏头文件
#include <rclcpp_components/register_node_macro.hpp>


// 定义自定义动作的命名空间
namespace custom_action_cpp
{
// 创建一个Fibonacci动作客户端类，继承自rclcpp::Node
class FibonacciActionClient : public rclcpp::Node
{
public:
  // 使用Fibonacci动作和目标句柄的别名
  using Fibonacci = custom_action_interfaces::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;


  // 构造函数，初始化节点
  explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
  : Node("fibonacci_action_client", options)
  {
    // 创建Fibonacci动作客户端
    this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
      this,
      "fibonacci");


    // 定义定时器回调函数，用于发送目标
    auto timer_callback_lambda = [this](){ return this->send_goal(); };
    // 创建定时器，每500毫秒调用一次回调函数
    this->timer_ = this->create_wall_timer(
      std::chrono::milliseconds(500),
      timer_callback_lambda);
  }


  // 发送目标的函数
  void send_goal()
  {
    // 使用std::placeholders命名空间
    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();
    }


    // 创建一个Fibonacci目标消息
    auto goal_msg = Fibonacci::Goal();
    // 设置目标阶数为10
    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 = [this](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");
      }
    };


    // 设置反馈回调函数
    send_goal_options.feedback_callback = [this](
      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());
    };


    // 设置结果回调函数
    send_goal_options.result_callback =  [this](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();
    };
    // 异步发送目标
    this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }


private:
  // 定义Fibonacci动作客户端的共享指针
  rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
  // 定义定时器的共享指针
  rclcpp::TimerBase::SharedPtr timer_;
};  // class FibonacciActionClient


}  // namespace custom_action_cpp


// 注册FibonacciActionClient节点
RCLCPP_COMPONENTS_REGISTER_NODE(custom_action_cpp::FibonacciActionClient)

前几行包括了我们编译所需的所有头文件。

接下来我们创建一个类，这个类是 rclcpp::Node 的派生类：

cpp 复制代码 
class FibonacciActionClient : public rclcpp::Node

FibonacciActionClient 类的构造函数将节点名称初始化为 fibonacci_action_client ：

cs 复制代码 
explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
  : Node("fibonacci_action_client", options)

构造函数还实例化了一个新的动作客户端：

kotlin 复制代码 
this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
      this,
      "fibonacci");

一个动作客户端需要三样东西：

  1. 模板化的动作类型名称： Fibonacci 。

  2. 一个 ROS 2 节点用于添加动作客户端到： this 。

  3. 动作名称： 'fibonacci' 。

我们还实例化了一个 ROS 定时器，它将启动对 send_goal 的唯一调用：

kotlin 复制代码 
auto timer_callback_lambda = [this](){ return this->send_goal(); };
    this->timer_ = this->create_wall_timer(
      std::chrono::milliseconds(500),
      timer_callback_lambda);

当计时器到期时，它将调用 send_goal ：

cpp 复制代码 
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 = [this](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");
      }
    };


    send_goal_options.feedback_callback = [this](
      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());
    };


    send_goal_options.result_callback = [this](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();
    };
    this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }

此函数执行以下操作：

  1. 取消计时器（因此它只被调用一次）。

  2. 等待动作服务器启动。

  3. 实例化一个新的 Fibonacci::Goal 。

  4. 设置响应、反馈和结果回调。

  5. 将目标发送到服务器。

当服务器接收并接受目标时，它会向客户端发送响应。该响应由 goal_response_callback 处理：

kotlin 复制代码 
// 设置目标响应回调函数
send_goal_options.goal_response_callback = [this](const GoalHandleFibonacci::SharedPtr & goal_handle)
{
  // 如果目标句柄不存在，说明目标被服务器拒绝
  if (!goal_handle) {
    // 输出错误日志，打印"Goal was rejected by server"
    RCLCPP_ERROR(this->get_logger(), "目标被服务器拒绝");
  } else {
    // 否则，输出信息日志，打印"Goal accepted by server, waiting for result"
    RCLCPP_INFO(this->get_logger(), "目标被服务器接受，等待结果");
  }
};

假设服务器接受了目标，它将开始处理。任何对客户端的反馈都将由 feedback_callback 处理：

cpp 复制代码 
// 设置反馈回调函数
send_goal_options.feedback_callback = [this](
      GoalHandleFibonacci::SharedPtr,
      const std::shared_ptr<const Fibonacci::Feedback> feedback)
{
  // 创建一个字符串流
  std::stringstream ss;
  // 向字符串流中添加字符串"Next number in sequence received: "
  ss << "接收到序列中的下一个数字：";
  // 遍历反馈序列中的每一个数字
  for (auto number : feedback->partial_sequence) {
    // 将数字添加到字符串流中
    ss << number << " ";
  }
  // 输出信息日志，打印反馈序列
  RCLCPP_INFO(this->get_logger(), ss.str().c_str());
};

当服务器处理完毕后，它会向客户端返回一个结果。结果由 result_callback 处理：

cpp 复制代码 
// 设置目标回调函数
send_goal_options.result_callback = [this](const GoalHandleFibonacci::WrappedResult & result)
{
  // 根据结果代码进行判断
  switch (result.code) {
    // 如果结果代码为SUCCEEDED（成功）
    case rclcpp_action::ResultCode::SUCCEEDED:
      // 不执行任何操作
      break;
    // 如果结果代码为ABORTED（中止）
    case rclcpp_action::ResultCode::ABORTED:
      // 输出错误日志，目标被中止
      RCLCPP_ERROR(this->get_logger(), "目标被中止");
      // 返回，不再执行后续代码
      return;
    // 如果结果代码为CANCELED（取消）
    case rclcpp_action::ResultCode::CANCELED:
      // 输出错误日志，目标被取消
      RCLCPP_ERROR(this->get_logger(), "目标被取消");
      // 返回，不再执行后续代码
      return;
    // 如果结果代码为其他值
    default:
      // 输出错误日志，未知的结果代码
      RCLCPP_ERROR(this->get_logger(), "未知的结果代码");
      // 返回，不再执行后续代码
      return;
  }
  // 创建一个字符串流
  std::stringstream ss;
  // 向字符串流中添加字符串"Result received: "
  ss << "接收到结果：";
  // 遍历结果序列中的每一个数字
  for (auto number : result.result->sequence) {
    // 将数字添加到字符串流中
    ss << number << " ";
  }
  // 输出信息日志，打印结果序列
  RCLCPP_INFO(this->get_logger(), ss.str().c_str());
  // 关闭rclcpp
  rclcpp::shutdown();
};

我们现在有了一个功能齐全的动作客户端。让我们开始构建并运行它。

3.2 编译动作客户端

在前一节中，我们已经将动作客户端代码放到了适当的位置。为了使其编译并运行，我们还需要做一些额外的工作。

首先我们需要设置 CMakeLists.txt，以便编译动作客户端。打开 custom_action_cpp/CMakeLists.txt ，并在 find_package 调用之后立即添加以下内容：

php 复制代码 
cmake_minimum_required(VERSION 3.8)  // 设置 CMake 的最低版本要求为 3.8
project(custom_action_cpp)  // 定义项目名称为 custom_action_cpp


if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")  // 如果编译器是 GNU C++ 或 Clang
  add_compile_options(-Wall -Wextra -Wpedantic)  // 添加编译选项：显示所有警告、额外警告和严格的编译检查
endif()


# find dependencies
# 查找依赖项
find_package(ament_cmake REQUIRED)  // 查找 ament_cmake 包
find_package(custom_action_interfaces REQUIRED)  // 查找 custom_action_interfaces 包
find_package(rclcpp REQUIRED)  // 查找 rclcpp 包
find_package(rclcpp_action REQUIRED)  // 查找 rclcpp_action 包
find_package(rclcpp_components REQUIRED)  // 查找 rclcpp_components 包


add_library(action_server SHARED  // 添加共享库 action_server
  src/fibonacci_action_server.cpp)  // 源文件为 src/fibonacci_action_server.cpp
target_include_directories(action_server PRIVATE  // 设置 action_server 的包含目录
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>  // 构建时的包含目录
  $<INSTALL_INTERFACE:include>)  // 安装时的包含目录
target_compile_definitions(action_server  // 设置 action_server 的编译定义
  PRIVATE "CUSTOM_ACTION_CPP_BUILDING_DLL")  // 定义 CUSTOM_ACTION_CPP_BUILDING_DLL
ament_target_dependencies(action_server  // 设置 action_server 的依赖项
  "custom_action_interfaces"  // 依赖 custom_action_interfaces
  "rclcpp"  // 依赖 rclcpp
  "rclcpp_action"  // 依赖 rclcpp_action
  "rclcpp_components")  // 依赖 rclcpp_components
rclcpp_components_register_node(action_server PLUGIN "custom_action_cpp::FibonacciActionServer" EXECUTABLE fibonacci_action_server)  // 注册节点 FibonacciActionServer 并生成可执行文件 fibonacci_action_server
install(TARGETS  // 安装目标
  action_server  // 安装 action_server
  ARCHIVE DESTINATION lib  // 安装静态库到 lib 目录
  LIBRARY DESTINATION lib  // 安装共享库到 lib 目录
  RUNTIME DESTINATION bin)  // 安装可执行文件到 bin 目录


add_library(action_client SHARED  // 添加共享库 action_client
  src/fibonacci_action_client.cpp)  // 源文件为 src/fibonacci_action_client.cpp
target_include_directories(action_client PRIVATE  // 设置 action_client 的包含目录
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>  // 构建时的包含目录
  $<INSTALL_INTERFACE:include>)  // 安装时的包含目录
target_compile_definitions(action_client  // 设置 action_client 的编译定义
  PRIVATE "CUSTOM_ACTION_CPP_BUILDING_DLL")  // 定义 CUSTOM_ACTION_CPP_BUILDING_DLL
ament_target_dependencies(action_client  // 设置 action_client 的依赖项
  "custom_action_interfaces"  // 依赖 custom_action_interfaces
  "rclcpp"  // 依赖 rclcpp
  "rclcpp_action"  // 依赖 rclcpp_action
  "rclcpp_components")  // 依赖 rclcpp_components
rclcpp_components_register_node(action_client PLUGIN "custom_action_cpp::FibonacciActionClient" EXECUTABLE fibonacci_action_client)  // 注册节点 FibonacciActionClient 并生成可执行文件 fibonacci_action_client
install(TARGETS  // 安装目标
  action_client  // 安装 action_client
  ARCHIVE DESTINATION lib  // 安装静态库到 lib 目录
  LIBRARY DESTINATION lib  // 安装共享库到 lib 目录
  RUNTIME DESTINATION bin)  // 安装可执行文件到 bin 目录


if(BUILD_TESTING)  // 如果启用了测试
  find_package(ament_lint_auto REQUIRED)  // 查找 ament_lint_auto 包
  # the following line skips the linter which checks for copyrights
  # 以下行跳过检查版权的 linter
  # comment the line when a copyright and license is added to all source files
  # 当所有源文件添加了版权和许可证时，注释掉此行
  set(ament_cmake_copyright_FOUND TRUE)  // 设置 ament_cmake_copyright_FOUND 为 TRUE
  # the following line skips cpplint (only works in a git repo)
  # 以下行跳过 cpplint（仅在 git 仓库中有效）
  # comment the line when this package is in a git repo and when
  # 当此包在 git 仓库中并且所有源文件添加了版权和许可证时，注释掉此行
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)  // 设置 ament_cmake_cpplint_FOUND 为 TRUE
  ament_lint_auto_find_test_dependencies()  // 自动查找测试依赖项
endif()


ament_package()  // 声明这是一个 ament 包

现在我们可以编译包了。转到 ros2_ws 的顶层，然后运行：

nginx 复制代码 
colcon build
cxy@ubuntu2404-cxy:~/ros2_ws$ colcon build --packages-select custom_action_cpp
Starting >>> custom_action_cpp
Finished <<< custom_action_cpp [6.69s]                     


Summary: 1 package finished [7.46s]

这应该编译整个工作空间，包括 custom_action_cpp 包中的 fibonacci_action_client 。

3.3 运行动作客户端

现在我们已经构建了动作客户端，我们可以运行它。首先确保一个动作服务器在一个单独的终端中运行。现在源代码工作区我们刚刚构建的 ( ros2_ws )，并尝试运行动作客户端：

nginx 复制代码 
ros2 run custom_action_cpp fibonacci_action_client

您应该会看到日志消息，记录了目标被接受、反馈被打印以及最终结果。

摘要

在本教程中，您将逐行组建一个 C++动作服务器和动作客户端，并配置它们以交换目标、反馈和结果。

相关内容

笔 记

cpp 复制代码 
rclcpp::Rate loop_rate(1);  // 设置循环频率为1Hz 
 loop_rate.sleep();  // 休眠一段时间
相关推荐
芊寻(嵌入式)7 分钟前
C转C++学习笔记--基础知识摘录总结
开发语言·c++·笔记·学习
獨枭9 分钟前
C++ 项目中使用 .dll 和 .def 文件的操作指南
c++
霁月风12 分钟前
设计模式——观察者模式
c++·观察者模式·设计模式
橘色的喵12 分钟前
C++编程：避免因编译优化引发的多线程死锁问题
c++·多线程·memory·死锁·内存屏障·内存栅栏·memory barrier
荒Huang13 分钟前
Linux挖矿病毒（kswapd0进程使cpu爆满）
linux·运维·服务器
一颗松鼠16 分钟前
JavaScript 闭包是什么？简单到看完就理解！
开发语言·前端·javascript·ecmascript
有梦想的咸鱼_18 分钟前
go实现并发安全hashtable 拉链法
开发语言·golang·哈希算法
海阔天空_201323 分钟前
Python pyautogui库：自动化操作的强大工具
运维·开发语言·python·青少年编程·自动化
桥田智能26 分钟前
气爪在自动化装配线中是如何应用的？
运维·自动化
天下皆白_唯我独黑30 分钟前
php 使用qrcode制作二维码图片
开发语言·php
热门推荐
01【HarmonyOS】HUAWEI DevEco Studio 下载地址汇总02(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明03组基轨迹建模 GBTM的介绍与实现（Stata 或 R）04【AIGC】重塑未来的科技巨轮05全面解析：构建基于深度学习的安全帽检测系统（UI界面+YOLO代码+数据集）06【经验分享】Ubuntu22.04安装微信(linux官方版)07基于YOLOv10深度学习的CT扫描图像肾结石智能检测系统【python源码+Pyqt5界面+数据集+训练代码】深度学习实战、目标检测08Ubuntu 20.04使用Livox mid 360 测试 FAST_LIO09RAG 实践- Ollama+RagFlow 部署本地知识库10【TC3xx芯片】TC3xx芯片电源管理系统PMS详解