编写动作服务器和客户端(C++)

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

辅导水平: 中级

时间 15 分钟

背景介绍

操作是 ROS 中的一种异步通信形式。 行动客户 将目标申请发送至 行动服务器. 行动服务器 将目标反馈和结果发送至 行动客户.

先决条件

您需要 自定义动作接口 软件包和 Fibonacci.action 接口、 创建行动.

任务

1 创建 custom_action_cpp 软件包

正如我们在 创建软件包 教程,我们需要创建一个新的软件包来保存我们的 C++ 和辅助代码。

1.1 创建 custom_action_cpp 软件包

进入在 上一个教程 (工作区),并为 C++ 操作服务器创建一个新软件包:

CD ~/ros2_ws/src ros2  创建 --依赖 自定义动作接口 rclcpp rclcpp_action rclcpp_components --许可证 Apache-2.0 -- 自定义动作

1.2 加入可见度控制

为了使软件包能在 Windows 上编译和运行,我们需要添加一些 "可见性控制"。更多详情,请参阅 Windows 提示和技巧文档中的 Windows 符号可见性.

打开 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
外部 "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 功能强大;
#include <内存>;
#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";

命名空间 自定义动作
{
 FibonacciActionServer :  rclcpp::节点
{
:
  使用 斐波那契 = 自定义动作接口::行动::斐波那契;
  使用 目标手柄斐波纳契 = rclcpp_action::服务器目标句柄<;斐波那契>;;

  自定义动作_cpp_public
  不含糊 FibonacciActionServer( rclcpp::节点选项 及样品; 选项 = rclcpp::节点选项())
  : 节点("fibonacci_action_server";, 选项)
  {
    使用 命名空间 标准::占位符;

    汽车 处理目标 = [](
       rclcpp_action::目标 UUID 及样品; uuid,
      标准::共享_ptr<; 斐波那契::目标>; 目标)
    {
      RCLCPP_INFO(->;get_logger(), "收到目标请求,订单 %d";, 目标->;订单);
      (空白)uuid;
      返回 rclcpp_action::目标响应::接受并执行;
    };

    汽车 处理取消 = [](
       标准::共享_ptr<;目标手柄斐波纳契>; 目标句柄)
    {
      RCLCPP_INFO(->;get_logger(), 收到取消目标的请求";);
      (空白)目标句柄;
      返回 rclcpp_action::取消响应::接受;
    };

    汽车 已接受 = [](
       标准::共享_ptr<;目标手柄斐波纳契>; 目标句柄)
    {
      // 需要快速返回,以避免阻塞执行器、
      // 因此,我们声明一个 lambda 函数,在新线程中调用
      汽车 在线程中执行 = [, 目标句柄](){返回 ->;执行(目标句柄);};
      标准::线程{在线程中执行}.脱离();
    };

    ->;行动服务器 = rclcpp_action::创建服务器<;斐波那契>;(
      ,
      "fibonacci";,
      处理目标,
      处理取消,
      已接受);
  }

私人:
  rclcpp_action::服务器<;斐波那契>::SharedPtr 行动服务器;

  空白 执行( 标准::共享_ptr<;目标手柄斐波纳契>; 目标句柄) {
    RCLCPP_INFO(->;get_logger(), "执行目标";);
    rclcpp::费率 循环速率(1);
     汽车 目标 = 目标句柄->;get_goal();
    汽车 反馈 = 标准::共享<;斐波那契::反馈意见>;();
    汽车 及样品; 顺序 = 反馈->;部分序列;
    顺序.推回(0);
    顺序.推回(1);
    汽车 结果 = 标准::共享<;斐波那契::结果>;();

    对于 (int i = 1; (i <; 目标->;订单) &&; rclcpp::好的(); ++i) {
      // 检查是否有取消请求
      如果 (目标句柄->;is_canceling()) {
        结果->;顺序 = 顺序;
        目标句柄->;已取消(结果);
        RCLCPP_INFO(->;get_logger(), 目标取消";);
        返回;
      }
      // 更新序列
      顺序.推回(顺序[i] + 顺序[i - 1]);
      // 发布反馈意见
      目标句柄->;发布反馈(反馈);
      RCLCPP_INFO(->;get_logger(), "发布反馈";);

      循环速率.睡眠();
    }

    // 检查目标是否完成
    如果 (rclcpp::好的()) {
      结果->;顺序 = 顺序;
      目标句柄->;继承(结果);
      RCLCPP_INFO(->;get_logger(), "目标成功";);
    }
  };

};  // 类 FibonacciActionServer

}  // 命名空间 custom_action_cpp

rclcpp_components_register_node(自定义动作::FibonacciActionServer)

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

接下来,我们创建一个从 rclcpp::Node:

 FibonacciActionServer :  rclcpp::节点

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

  不含糊 FibonacciActionServer( rclcpp::节点选项 及样品; 选项 = rclcpp::节点选项())
  : 节点("fibonacci_action_server";, 选项)

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

      循环速率.睡眠();
    }

    // 检查目标是否完成
    如果 (rclcpp::好的()) {

行动服务器需要具备 6 个条件:

  1. 模板化操作类型名称: 斐波那契.

  2. 要添加操作的 ROS 2 节点: .

  3. 行动名称: 斐波那契.

  4. 用于处理目标的回调函数: 处理目标

  5. 用于处理取消的回调函数: 处理取消.

  6. 用于处理目标接受的回调函数: 接受.

各种回调的实现都是通过 [lambda 表达式] (https://en.cppreference.com/w/cpp/language/lambda) 在构造函数中。需要注意的是,所有的回调都需要快速返回,否则就有可能使执行器处于饥饿状态。

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

    汽车 处理目标 = [](
       rclcpp_action::目标 UUID 及样品; uuid,
      标准::共享_ptr<; 斐波那契::目标>; 目标)
    {
      RCLCPP_INFO(->;get_logger(), "收到目标请求,订单 %d";, 目标->;订单);
      (空白)uuid;
      返回 rclcpp_action::目标响应::接受并执行;
    };

这种实现方式只接受所有目标。

接下来是处理取消订单的回调:

    汽车 处理取消 = [](
       标准::共享_ptr<;目标手柄斐波纳契>; 目标句柄)
    {
      RCLCPP_INFO(->;get_logger(), 收到取消目标的请求";);
      (空白)目标句柄;
      返回 rclcpp_action::取消响应::接受;
    };

这种实现方式只是告诉客户端它接受了取消。

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

    汽车 已接受 = [](
       标准::共享_ptr<;目标手柄斐波纳契>; 目标句柄)
    {
      // 需要快速返回,以避免阻塞执行器、
      // 因此,我们声明一个 lambda 函数,在新线程中调用
      汽车 在线程中执行 = [, 目标句柄](){返回 ->;执行(目标句柄);};
      标准::线程{在线程中执行}.脱离();
    };

由于执行的是一个长期运行的操作,我们会产生一个线程来执行实际工作,并从 已接受 很快。

所有进一步的处理和更新都在 执行 方法:

  空白 执行( 标准::共享_ptr<;目标手柄斐波纳契>; 目标句柄) {
    RCLCPP_INFO(->;get_logger(), "执行目标";);
    rclcpp::费率 循环速率(1);
     汽车 目标 = 目标句柄->;get_goal();
    汽车 反馈 = 标准::共享<;斐波那契::反馈意见>;();
    汽车 及样品; 顺序 = 反馈->;部分序列;
    顺序.推回(0);
    顺序.推回(1);
    汽车 结果 = 标准::共享<;斐波那契::结果>;();

    对于 (int i = 1; (i <; 目标->;订单) &&; rclcpp::好的(); ++i) {
      // 检查是否有取消请求
      如果 (目标句柄->;is_canceling()) {
        结果->;顺序 = 顺序;
        目标句柄->;已取消(结果);
        RCLCPP_INFO(->;get_logger(), 目标取消";);
        返回;
      }
      // 更新序列
      顺序.推回(顺序[i] + 顺序[i - 1]);
      // 发布反馈意见
      目标句柄->;发布反馈(反馈);
      RCLCPP_INFO(->;get_logger(), "发布反馈";);

      循环速率.睡眠();
    }

    // 检查目标是否完成
    如果 (rclcpp::好的()) {
      结果->;顺序 = 顺序;
      目标句柄->;继承(结果);
      RCLCPP_INFO(->;get_logger(), "目标成功";);
    }
  };

该工作线程每秒处理一个斐波那契数列的序列号,每一步都会发布反馈更新。完成处理后,它将 目标句柄 并退出。

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

2.2 编译行动服务器

在上一节中,我们将动作服务器代码安装到位。为了让它编译和运行,我们还需要做几件事。

首先,我们需要设置 CMakeLists.txt,以便编译动作服务器。打开 custom_action_cpp/CMakeLists.txt之后添加以下内容 查找软件包 电话

add_library(行动服务器 共享
  src/fibonacci_action_server.cpp)
目标包含目录(行动服务器 私人
  $<;BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>;
  $<;INSTALL_INTERFACE:include>;)
目标编译定义(行动服务器
  私人 "CUSTOM_ACTION_CPP_BUILDING_DLL";)
ament_target_dependencies(行动服务器
  "custom_action_interfaces";
  "rclcpp";
  "rclcpp_action";
  "rclcpp_components";)
rclcpp_components_register_node(行动服务器 插件 "custom_action_cpp::FibonacciActionServer"; 可执行 fibonacci_action_server)
安装(目标
  行动服务器
  存档 目的地 lib
  图书馆 目的地 lib
  运行时间 目的地 箱柜)

现在我们可以编译软件包了。转到 ros2_ws然后运行:

胶管 构建

这将编译整个工作区,包括 fibonacci_action_server自定义动作 包装

2.3 运行行动服务器

现在我们已经构建了行动服务器,可以运行它了。将我们刚刚构建的工作区(ros2_ws),并尝试运行动作服务器:

玫瑰2 运行 自定义动作 fibonacci_action_server

3 撰写行动客户

3.1 编写动作客户端代码

打开 custom_action_cpp/src/fibonacci_action_client.cpp并输入以下代码:

#include 功能强大;
#include 未来;
#include <内存>;
#include <字符串>;
#include 流>;

#include "custom_action_interfaces/action/fibonacci.hpp";

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

命名空间 自定义动作
{
 FibonacciActionClient :  rclcpp::节点
{
:
  使用 斐波那契 = 自定义动作接口::行动::斐波那契;
  使用 目标手柄斐波纳契 = rclcpp_action::客户目标句柄<;斐波那契>;;

  不含糊 FibonacciActionClient( rclcpp::节点选项 及样品; 选项)
  : 节点("fibonacci_action_client";, 选项)
  {
    ->;客户端ptr_ = rclcpp_action::创建客户端<;斐波那契>;(
      ,
      "fibonacci";);

    汽车 timer_callback_lambda = [](){ 返回 ->;发送目标(); };
    ->;定时器 = ->;创建隔离墙计时器(
      标准::计时器::毫秒数(500),
      timer_callback_lambda);
  }

  空白 发送目标()
  {
    使用 命名空间 标准::占位符;

    ->;定时器->;取消();

    如果 (!->;客户端ptr_->;等待行动服务器()) {
      RCLCPP_ERROR(->;get_logger(), "等待后行动服务器不可用";);
      rclcpp::关闭();
    }

    汽车 goal_msg = 斐波那契::目标();
    goal_msg.订单 = 10;

    RCLCPP_INFO(->;get_logger(), 发送目标";);

    汽车 发送目标选项 = rclcpp_action::客户<;斐波那契>::发送目标选项();
    发送目标选项.目标响应回调 = []( 目标手柄斐波纳契::SharedPtr 及样品; 目标句柄)
    {
      如果 (!目标句柄) {
        RCLCPP_ERROR(->;get_logger(), 目标被服务器拒绝";);
      } 不然 {
        RCLCPP_INFO(->;get_logger(), 服务器接受目标,等待结果";);
      }
    };

    发送目标选项.反馈回调 = [](
      目标手柄斐波纳契::SharedPtr,
       标准::共享_ptr<; 斐波那契::反馈意见>; 反馈)
    {
      标准::字符串流 ss;
      ss <<; "收到序列中的下一个号码:";;
      对于 (汽车 编号 : 反馈->;部分序列) {
        ss <<; 编号 <<; " ";;
      }
      RCLCPP_INFO(->;get_logger(), ss.字符串().c_str());
    };

    发送目标选项.result_callback = []( 目标手柄斐波纳契::包裹结果 及样品; 结果)
    {
      开关 (结果.代码) {
        个案 rclcpp_action::结果代码::成功:
          断裂;
        个案 rclcpp_action::结果代码::中止:
          RCLCPP_ERROR(->;get_logger(), 目标失败";);
          返回;
        个案 rclcpp_action::结果代码::取消:
          RCLCPP_ERROR(->;get_logger(), 目标被取消";);
          返回;
        默认:
          RCLCPP_ERROR(->;get_logger(), 未知结果代码";);
          返回;
      }
      标准::字符串流 ss;
      ss <<; 已收到结果: ";;
      对于 (汽车 编号 : 结果.结果->;顺序) {
        ss <<; 编号 <<; " ";;
      }
      RCLCPP_INFO(->;get_logger(), ss.字符串().c_str());
      rclcpp::关闭();
    };
    ->;客户端ptr_->;异步发送目标(goal_msg, 发送目标选项);
  }

私人:
  rclcpp_action::客户<;斐波那契>::SharedPtr 客户端ptr_;
  rclcpp::定时器基数::SharedPtr 定时器;
};  // 类 FibonacciActionClient

}  // 命名空间 custom_action_cpp

rclcpp_components_register_node(自定义动作::FibonacciActionClient)

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

接下来,我们创建一个从 rclcpp::Node:

 FibonacciActionClient :  rclcpp::节点

的构造函数 FibonacciActionClient 类将节点名称初始化为 斐波那契行动客户端:

  不含糊 FibonacciActionClient( rclcpp::节点选项 及样品; 选项)
  : 节点("fibonacci_action_client";, 选项)

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

    ->;客户端ptr_ = rclcpp_action::创建客户端<;斐波那契>;(
      ,
      "fibonacci";);

行动客户需要具备 3 个条件:

  1. 模板化操作类型名称: 斐波那契.

  2. 要添加动作客户端的 ROS 2 节点: .

  3. 行动名称: 斐波那契.

我们还实例化了一个 ROS 定时器,它将启动对 发送目标:

    汽车 timer_callback_lambda = [](){ 返回 ->;发送目标(); };
    ->;定时器 = ->;创建隔离墙计时器(
      标准::计时器::毫秒数(500),
      timer_callback_lambda);

计时器到期时,它会调用 发送目标:

  空白 发送目标()
  {
    使用 命名空间 标准::占位符;

    ->;定时器->;取消();

    如果 (!->;客户端ptr_->;等待行动服务器()) {
      RCLCPP_ERROR(->;get_logger(), "等待后行动服务器不可用";);
      rclcpp::关闭();
    }

    汽车 goal_msg = 斐波那契::目标();
    goal_msg.订单 = 10;

    RCLCPP_INFO(->;get_logger(), 发送目标";);

    汽车 发送目标选项 = rclcpp_action::客户<;斐波那契>::发送目标选项();
    发送目标选项.目标响应回调 = []( 目标手柄斐波纳契::SharedPtr 及样品; 目标句柄)
    {
      如果 (!目标句柄) {
        RCLCPP_ERROR(->;get_logger(), 目标被服务器拒绝";);
      } 不然 {
        RCLCPP_INFO(->;get_logger(), 服务器接受目标,等待结果";);
      }
    };

    发送目标选项.反馈回调 = [](
      目标手柄斐波纳契::SharedPtr,
       标准::共享_ptr<; 斐波那契::反馈意见>; 反馈)
    {
      标准::字符串流 ss;
      ss <<; "收到序列中的下一个号码:";;
      对于 (汽车 编号 : 反馈->;部分序列) {
        ss <<; 编号 <<; " ";;
      }
      RCLCPP_INFO(->;get_logger(), ss.字符串().c_str());
    };

    发送目标选项.result_callback = []( 目标手柄斐波纳契::包裹结果 及样品; 结果)
    {
      开关 (结果.代码) {
        个案 rclcpp_action::结果代码::成功:
          断裂;
        个案 rclcpp_action::结果代码::中止:
          RCLCPP_ERROR(->;get_logger(), 目标失败";);
          返回;
        个案 rclcpp_action::结果代码::取消:
          RCLCPP_ERROR(->;get_logger(), 目标被取消";);
          返回;
        默认:
          RCLCPP_ERROR(->;get_logger(), 未知结果代码";);
          返回;
      }
      标准::字符串流 ss;
      ss <<; 已收到结果: ";;
      对于 (汽车 编号 : 结果.结果->;顺序) {
        ss <<; 编号 <<; " ";;
      }
      RCLCPP_INFO(->;get_logger(), ss.字符串().c_str());
      rclcpp::关闭();
    };
    ->;客户端ptr_->;异步发送目标(goal_msg, 发送目标选项);
  }

该功能执行以下操作

  1. 取消计时器(因此只调用一次)。

  2. 等待行动服务器出现。

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

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

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

服务器接收并接受目标后,会向客户端发送一个响应。该响应由 目标响应回调:

    发送目标选项.目标响应回调 = []( 目标手柄斐波纳契::SharedPtr 及样品; 目标句柄)
    {
      如果 (!目标句柄) {
        RCLCPP_ERROR(->;get_logger(), 目标被服务器拒绝";);
      } 不然 {
        RCLCPP_INFO(->;get_logger(), 服务器接受目标,等待结果";);
      }
    };

假设服务器接受了目标,就会开始处理。对客户端的任何反馈都将由 反馈回调:

    发送目标选项.反馈回调 = [](
      目标手柄斐波纳契::SharedPtr,
       标准::共享_ptr<; 斐波那契::反馈意见>; 反馈)
    {
      标准::字符串流 ss;
      ss <<; "收到序列中的下一个号码:";;
      对于 (汽车 编号 : 反馈->;部分序列) {
        ss <<; 编号 <<; " ";;
      }
      RCLCPP_INFO(->;get_logger(), ss.字符串().c_str());
    };

服务器处理完毕后,会向客户端返回一个结果。结果由 result_callback:

    发送目标选项.result_callback = []( 目标手柄斐波纳契::包裹结果 及样品; 结果)
    {
      开关 (结果.代码) {
        个案 rclcpp_action::结果代码::成功:
          断裂;
        个案 rclcpp_action::结果代码::中止:
          RCLCPP_ERROR(->;get_logger(), 目标失败";);
          返回;
        个案 rclcpp_action::结果代码::取消:
          RCLCPP_ERROR(->;get_logger(), 目标被取消";);
          返回;
        默认:
          RCLCPP_ERROR(->;get_logger(), 未知结果代码";);
          返回;
      }
      标准::字符串流 ss;
      ss <<; 已收到结果: ";;
      对于 (汽车 编号 : 结果.结果->;顺序) {
        ss <<; 编号 <<; " ";;
      }
      RCLCPP_INFO(->;get_logger(), ss.字符串().c_str());
      rclcpp::关闭();
    };

我们现在有了一个功能完备的行动客户端。让我们构建并运行它。

3.2 编译行动客户端

在上一节中,我们将动作客户端代码安装到位。为了让它编译和运行,我们还需要做几件事。

首先,我们需要设置 CMakeLists.txt,以便编译动作客户端。打开 custom_action_cpp/CMakeLists.txt之后添加以下内容 查找软件包 电话

add_library(动作客户端 共享
  src/fibonacci_action_client.cpp)
目标包含目录(动作客户端 私人
  $<;BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>;
  $<;INSTALL_INTERFACE:include>;)
目标编译定义(动作客户端
  私人 "CUSTOM_ACTION_CPP_BUILDING_DLL";)
ament_target_dependencies(动作客户端
  "custom_action_interfaces";
  "rclcpp";
  "rclcpp_action";
  "rclcpp_components";)
rclcpp_components_register_node(动作客户端 插件 "custom_action_cpp::FibonacciActionClient"; 可执行 斐波那契行动客户端)
安装(目标
  动作客户端
  存档 目的地 lib
  图书馆 目的地 lib
  运行时间 目的地 箱柜)

现在我们可以编译软件包了。转到 ros2_ws然后运行:

胶管 构建

这将编译整个工作区,包括 斐波那契行动客户端自定义动作 包装

3.3 运行行动客户端

现在我们已经创建了动作客户端,可以运行它了。首先,确保动作服务器在单独的终端中运行。现在,将我们刚刚构建的工作区 (ros2_ws),并尝试运行动作客户端:

玫瑰2 运行 自定义动作 斐波那契行动客户端

你应该能看到目标被接受、反馈被打印以及最终结果的记录信息。

摘要

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