ROS2 Jazzy：用C++实现一个动作服务器和客户端

目标

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

背景信息

动作是ROS2中的一种异步通信方式。动作客户端向动作服务器发送目标请求，动作服务器向动作客户端发送目标反馈和结果。本文使用了可组合节点，关于组合会在后续的文章中讲解。

先决条件

• 已安装ROS2和colcon。
• 了解如何设置工作空间和创建软件包。
• 操作前请先加载ROS2环境。

创建动作接口

1. 创建接口包

mkdir -p ~/ros2_ws/src
cd ~/ros2_ws/src
ros2 pkg create --license Apache-2.0 custom_action_interfaces

2. 定义动作

动作通过扩展名为.action的文件定义，格式如下：

# Request
---
# Result
---
# Feedback

一个动作定义由三部分消息定义组成，用---分隔：

• 请求消息：由动作客户端发送给动作服务器，用于发起新目标。
• 结果消息：当目标完成时，由动作服务器发送给动作客户端。
• 反馈消息：动作服务器定期发送给动作客户端，提供目标的进度更新。 动作的一个实例通常称为一个“目标”。

假设我们要定义一个名为“Fibonacci”的新动作，用于计算斐波那契数列。

在custom_action_interfaces包中创建动作目录：

cd custom_action_interfaces
mkdir action

在action目录下创建Fibonacci.action文件，内容如下：

int32 order    # 目标请求：要计算的斐波那契数列的项数
---
int32[] sequence    # 结果：最终生成的数列
---
int32[] partial_sequence    # 反馈：当前已生成的部分数列

3. 构建动作

在代码中使用新定义的“Fibonacci”动作类型前，必须将动作定义传递给rosidl代码生成流程。

3.1：修改CMakeLists.txt

在ament_package()语句前添加以下内容：

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "action/Fibonacci.action"
)

3.2：修改package.xml

添加所需依赖：

<buildtool_depend>rosidl_default_generators</buildtool_depend>
<member_of_group>rosidl_interface_packages</member_of_group>

3.3：构建软件包

# 切换到工作空间根目录
cd ~/ros2_ws
# 构建
colcon build --packages-select custom_action_interfaces

3.4：验证动作定义

加载工作空间环境：

source install/local_setup.bash

检查动作定义是否存在：

ros2 interface show custom_action_interfaces/action/Fibonacci

你应能在终端看到Fibonacci动作的定义内容。

实现动作服务器和客户端

1. 创建custom_action_cpp包

1.1 创建包

进入动作工作空间（记得先加载工作空间环境），为C++动作服务器创建新包：

# Linux/macOS/Windows
cd ~/ros2_ws/src
ros2 pkg create --dependencies custom_action_interfaces rclcpp rclcpp_action rclcpp_components --license Apache-2.0 custom_action_cpp

1.2 添加可见性控制(visibility control)

为了使包在Windows上编译运行，需要添加“可见性控制visibility control“(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，粘贴以下代码：

#ifndef CUSTOM_ACTION_CPP__VISIBILITY_CONTROL_H_
#define CUSTOM_ACTION_CPP__VISIBILITY_CONTROL_H_

#ifdef __cplusplus
extern "C"
{
#endif

// 此逻辑（同时命名空间化）借鉴自gcc wiki上的示例：
//     https://gcc.gnu.org/wiki/Visibility

#if defined _WIN32 || defined __CYGWIN__
  #ifdef __GNUC__
    #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
    #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
    #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
}
#endif

#endif  // CUSTOM_ACTION_CPP__VISIBILITY_CONTROL_H_

2. 编写动作服务器

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"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"

#include "custom_action_cpp/visibility_control.h"

namespace custom_action_cpp
{
class FibonacciActionServer : public rclcpp::Node
{
public:
  using Fibonacci = custom_action_interfaces::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;

  CUSTOM_ACTION_CPP_PUBLIC
  explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node("fibonacci_action_server", options)
  {
    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;
      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;
      return rclcpp_action::CancelResponse::ACCEPT;
    };

    auto handle_accepted = [this](
      const std::shared_ptr<GoalHandleFibonacci> goal_handle)
    {
      // 由于执行是一个长时间运行的操作，我们会派生一个线程来执行实际工作，并从 handle_accepted 回调中快速返回。
      auto execute_in_thread = [this, goal_handle](){return this->execute(goal_handle);};
      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);
    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) {
      // 检查是否有取消请求
      if (goal_handle->is_canceling()) {
        result->sequence = sequence;
        goal_handle->canceled(result);
        RCLCPP_INFO(this->get_logger(), "Goal canceled");
        return;
      }
      // 更新数列
      sequence.push_back(sequence[i] + sequence[i - 1]);
      // 发布反馈
      goal_handle->publish_feedback(feedback);
      RCLCPP_INFO(this->get_logger(), "Publish feedback");

      loop_rate.sleep();
    }

    // 检查目标是否完成
    if (rclcpp::ok()) {
      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)

代码说明：

• 动作服务器类继承自rclcpp::Node，构造函数中通过rclcpp_action::create_server创建动作服务器，需传入动作类型、节点指针、动作名称及三个回调函数（处理目标、取消、接受）。
• handle_goal回调简单接受所有目标；handle_cancel回调接受取消请求；handle_accepted回调在新线程中执行目标计算逻辑（execute方法）。
• execute方法每秒计算一次斐波那契数列，发布反馈，完成后返回结果或处理取消请求。

2.2 编译动作服务器

修改custom_action_cpp/CMakeLists.txt，在find_package后添加：

add_library(action_server SHARED
  src/fibonacci_action_server.cpp)
target_include_directories(action_server PRIVATE
  lt;BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  lt;INSTALL_INTERFACE:include>)
target_compile_definitions(action_server
  PRIVATE "CUSTOM_ACTION_CPP_BUILDING_DLL")
ament_target_dependencies(action_server
  "custom_action_interfaces"
  "rclcpp"
  "rclcpp_action"
  "rclcpp_components")
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)

在工作空间根目录执行编译：

colcon build

2.3 运行动作服务器

ros2 run custom_action_cpp fibonacci_action_server

3. 编写动作客户端

3.1 编写动作客户端代码

打开custom_action_cpp/src/fibonacci_action_client.cpp，粘贴以下代码：

#include <functional>
#include <future>
#include <memory>
#include <string>
#include <sstream>

#include "custom_action_interfaces/action/fibonacci.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"

namespace custom_action_cpp
{
class FibonacciActionClient : public rclcpp::Node
{
public:
  using Fibonacci = custom_action_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");

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

  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);
  }

private:
  rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
  rclcpp::TimerBase::SharedPtr timer_;
};  // class FibonacciActionClient

}  // namespace custom_action_cpp

RCLCPP_COMPONENTS_REGISTER_NODE(custom_action_cpp::FibonacciActionClient)

代码说明：

• 动作客户端类同样继承自rclcpp::Node，构造函数中通过rclcpp_action::create_client创建客户端，需传入动作类型、节点指针、动作名称。
• 定时器触发send_goal方法，该方法等待动作服务器启动，设置目标参数（计算10项斐波那契数列），并注册三个回调函数（处理目标响应、反馈、结果）。
• goal_response_callback处理服务器是否接受目标；feedback_callback打印每一步反馈；result_callback处理最终结果并关闭节点。

2.2 编译动作客户端

修改custom_action_cpp/CMakeLists.txt，在find_package后添加：

add_library(action_client SHARED
  src/fibonacci_action_client.cpp)
target_include_directories(action_client PRIVATE
  lt;BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  lt;INSTALL_INTERFACE:include>)
target_compile_definitions(action_client
  PRIVATE "CUSTOM_ACTION_CPP_BUILDING_DLL")
ament_target_dependencies(action_client
  "custom_action_interfaces"
  "rclcpp"
  "rclcpp_action"
  "rclcpp_components")
rclcpp_components_register_node(action_client PLUGIN "custom_action_cpp::FibonacciActionClient" EXECUTABLE fibonacci_action_client)
install(TARGETS
  action_client
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin)

再次编译工作空间：

colcon build

2.3 运行动作客户端

先确保动作服务器已在另一终端运行，然后执行：

ros2 run custom_action_cpp fibonacci_action_client

你将看到目标被接受、反馈逐步打印、最终结果输出的日志信息。

总结

本教程逐步实现了C++动作服务器和客户端，并配置它们交换目标、反馈和结果。通过lambda表达式处理异步回调，利用多线程避免阻塞执行器，完整展示了ROS2中动作通信的核心流程。

关注【智践行】公众号，发送 【机器人】 获得机器人经典学习资料