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

本文链接：https://blog.youkuaiyun.com/cxyhjl/article/details/140282253

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

教程级别：中级

时间：15 分钟

背景
先决条件
任务
- 1. 创建 custom_action_cpp 包
- 2. 编写动作服务器
- 3. 编写动作客户端
摘要
相关内容

背景

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

先决条件

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

任务

1. 创建 custom_action_cpp 包

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

1.1 创建 custom_action_cpp 包

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

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 ，然后输入以下代码：

cxy@ubuntu2404-cxy:~/ros2_ws/src/custom_action_cpp$ gedit include/custom_action_cpp/visibility_control.h

#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 ，然后输入以下代码：

#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 &#