首页 > 编程语言 >【ROS2】中级-编写动作服务器和客户端（C++）

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

时间：2024-07-10 18:57:30浏览次数：18  
标签：goal rclcpp C++ custom cpp action server ROS2 客户端

目标：用 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 <[email protected]>']
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 = 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 的派生类：

class FibonacciActionServer : public rclcpp::Node

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

explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node("fibonacci_action_server", options)

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

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 表达式完成的。请注意，所有回调都需要快速返回，否则我们冒着使执行器饥饿的风险。

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

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)
    {
      // 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 方法中完成：

// 定义执行函数，参数为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 调用之后立即添加以下内容：

// 添加一个名为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 的顶层，然后运行：

colcon build

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

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 ），并尝试运行动作服务器：

ros2 run custom_action_cpp fibonacci_action_server

3. 编写动作客户端

3.1 编写动作客户端代码 

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

// 包含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 的派生类：

class FibonacciActionClient : public rclcpp::Node

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

explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
  : Node("fibonacci_action_client", options)

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

this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
      this,
      "fibonacci");

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

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

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

  3. 动作名称： 'fibonacci' 。

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

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

当计时器到期时，它将调用 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 = [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 处理：

// 设置目标响应回调函数
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 处理：

// 设置反馈回调函数
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 处理：

// 设置目标回调函数
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 调用之后立即添加以下内容：

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 的顶层，然后运行：

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 )，并尝试运行动作客户端：

ros2 run custom_action_cpp fibonacci_action_client

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

57378f4fa5ec124c4e3fb0b54bd63300.png

 摘要

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

 相关内容 

  • 您可以通过几种方式用 C++编写动作服务器和客户端；请查看 ros2/examples 仓库 https://github.com/ros2/examples/tree/jazzy/rclcpp 中的 minimal_action_server 和 minimal_action_client 包。

  • 有关 ROS 操作的更详细信息，请参阅设计文章https://design.ros2.org/articles/actions.html 。

笔 记

rclcpp::Rate loop_rate(1);  // 设置循环频率为1Hz 
 loop_rate.sleep();  // 休眠一段时间

428eb8d4cad467482a68aed5bc832b62.png

637811629d3ec19fb8b0539e0a1dd2e3.png

标签：goal,rclcpp,C++,custom,cpp,action,server,ROS2,客户端
From： https://blog.csdn.net/cxyhjl/article/details/140282253

相关文章

  • C++中各类常用算法的总结以及使用
    1.常用算法文章目录1.常用算法1.常用遍历算法1.for_each2.transform2.常用查找算法1.find2.find_if3.adjacent_find4.binary_search5.count6.count_if3.常用排序算法1.sort2.random_shuffle3.merge4.reverse4.常用拷贝和替换算法1.copy2.replace3.repla......
  • 【C++】14.多态
    一、多态的概念在编程与现实的映射中就是，不同的对象完成相同的行为而产生的不同状态。举个栗子：比如买票这个行为，当普通人买票时，是全价买票；学生买票时，是半价买票；军人买票时是优先买票。再举个栗子：动物的叫声，猫的叫声是“喵喵”；狗的叫声是“汪汪”；老虎的叫声是“劳资蜀道山......
  • 【NOI】C++算法设计入门之贪心
    文章目录前言一、概念1.导入2.概念2.1贪心算法的核心思想2.2贪心算法的步骤2.3贪心算法的应用场景二、例题讲解问题：1372.活动选择问题：1456.淘淘捡西瓜问题：1551-任务调度问题：1561.买木头三、总结五、感谢前言贪心算法，如同成语"得陇望蜀"所描述的那样，总是......
  • 深入剖析C++的 “属性“（Attribute specifier sequence）
    引言在阅读开源项目源代码是，发现了一个有趣且特殊的C++特性：属性。属性（attributespecifiersequences）是在C++11标准引入的。在C++11之前，编译器特有的扩展被广泛用来提供额外的代码信息。例如，GNU编译器（GCC）使用__attribute__来控制函数的行为。但是缺点也很明显，那就是这种方......
  • c++新特性
    1.c++17新特性[[fallthrough]] 属性：这个属性用于在switch语句中。通常，当switch语句的一个case执行完毕后，会自动跳转到switch语句的末尾，除非存在break语句。如果你想要故意从一个case“落入”（fallthrough）到下一个case，可以使用[[fallthrough]]属性来告诉编译器这是故意的行为，以......
  • UE C++获取本机IP地址
    UEC++获取本机IP地址开发环境UE5.2.1VisualStudio201916.11.19代码部分XXX.Build.cs文件添加以下依赖：PrivateDependencyModuleNames.AddRange(newString[]{"Sockets"}).cpp文件：#include"SockerSubsystem.h"voidGetIp(){ boolbBind=false; TSharedRef......
  • C++使用宏来判断当前编译期支持的C++标准版本的方法
       需要注意的是，很多时候，某个编译器的版本并不完整支持某个C++标准，比如VisualStudio2010SP1，虽然支持了部分C++11的能力，但是依然有很多C++11的特性是不支持的。因此单纯通过C++标准的版本号来鉴别C++特性是否可用是并不完备的方法。具体支持情况可以参考这里。#ifdef_......
  • 《C++20设计模式》策略模式
    文章目录一、前言二、实现1、UML类图2、实现一、前言策略模式和状态模式实现方式一样，只是目的不同，如果会状态模式的这个也就可以随便看看啦。相关代码可以在这里，如有帮助给个star！AidenYuanDev/design_patterns_in_modern_Cpp_20二、实现1、UML类图2、实现#in......
  • 《C++设计模式》状态模式
    文章目录一、前言二、实现一、UML类图二、实现一、前言状态模式理解最基本上的我觉得应该也是够用了，实际用的话，也应该用的是Boost.MSM状态机。相关代码可以在这里，如有帮助给个star！AidenYuanDev/design_patterns_in_modern_Cpp_20二、实现一、UML类图二、实现#......
  • 《C++20设计模式》观察者模式
    一、前言观察者模式感觉真的很难，我这里就实现书上差不多的例子，供大家学习吧！相关代码可以在这里，如有帮助给个star！AidenYuanDev/design_patterns_in_modern_Cpp_20二、实现这里说一下观察者模式是干什么的。在实际中很有用，比如以下的例子。当股票价格变化时，自动通知所有......

赞助商

阅读排行

网站主页关于我们联系我们网站地图

本网站内容转载自其他媒体，侵权联系[admin##ips99.com]。

Copyright © 2020-2023 IPS99 版权所有 IPS99