ROS2学习笔记21--编写action服务器和客户端（C++）

Posted 2021-08-30 鸿_H

概要：这篇内容主要介绍如何使用C++来编写动作服务器和客户端

环境：ubuntu20.04,ros2-foxy,vscode

最后如果没有陈述实操过程中碰到问题的话，则表示该章节都可被本人正常复现．

3.2编写动作服务器和客户端（C++）（原文：https://docs.ros.org/en/foxy/Tutorials/Actions/Writing-a-Cpp-Action-Server-Client.html）

>>教程>>编写动作服务器和客户端（C++）

你正阅读的是ros2较老版本(Foxy)，但仍然支持的说明文档．想查看最新版本的信息，请看galactic版本链接（ https://docs.ros.org/en/galactic/Tutorials.html）

编写动作服务器和客户端（C++）

目标：实现c++版的动作服务器和客户端．

时长：15min

目录

1.背景
2.预备知识
3.步骤
3.1创建action_tutorials_cpp包
3.2编写动作服务器
3.3编写动作客户端
4.总结
5.相关内容

1.背景

动作是ros异步通信的方式．动作客户端发送目标请求到动作服务器，动作服务器发送目标反馈以及结果到动作客户端．

2.预备知识

你需要前面创建一个动作教程里面创建的action_tutorials_interfaces包以及Fibonacci.action接口．

3.步骤

3.1创建action_tutorials_cpp包

参考创建第一个ros2包课程，我们需要创建一个包来放c++源码的，并提供支持的．

3.1.1创建包action_tutorials_cpp package

进入前面课程创建动作的工作空间（记得source一下环境变量），然后创建一个装c++动作服务器的包：

linux:

cd ~/action_ws/src
ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp

(本人练习时，还是在之前dev_ws工作空间，所以进入工作空间会跟官方有点出入)

3.1.2添加明显需要内容

为了保证包的编译以及在窗口有效，我们需要添加明显需要的内容．为啥需要这些的细节缘由，可以看这里（https://docs.microsoft.com/en-us/cpp/cpp/dllexport-dllimport?view=msvc-160）．

打开并创建路径文件action_tutorials_cpp/include/action_tutorials_cpp/visibility_control.h，并复制一下代码进去：

#ifndef ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_
#define ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_

#ifdef __cplusplus
extern "C"
{
#endif

// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
//     https://gcc.gnu.org/wiki/Visibility

#if defined _WIN32 || defined __CYGWIN__
  #ifdef __GNUC__
    #define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((dllexport))
    #define ACTION_TUTORIALS_CPP_IMPORT __attribute__ ((dllimport))
  #else
    #define ACTION_TUTORIALS_CPP_EXPORT __declspec(dllexport)
    #define ACTION_TUTORIALS_CPP_IMPORT __declspec(dllimport)
  #endif
  #ifdef ACTION_TUTORIALS_CPP_BUILDING_DLL
    #define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_EXPORT
  #else
    #define ACTION_TUTORIALS_CPP_PUBLIC ACTION_TUTORIALS_CPP_IMPORT
  #endif
  #define ACTION_TUTORIALS_CPP_PUBLIC_TYPE ACTION_TUTORIALS_CPP_PUBLIC
  #define ACTION_TUTORIALS_CPP_LOCAL
#else
  #define ACTION_TUTORIALS_CPP_EXPORT __attribute__ ((visibility("default")))
  #define ACTION_TUTORIALS_CPP_IMPORT
  #if __GNUC__ >= 4
    #define ACTION_TUTORIALS_CPP_PUBLIC __attribute__ ((visibility("default")))
    #define ACTION_TUTORIALS_CPP_LOCAL  __attribute__ ((visibility("hidden")))
  #else
    #define ACTION_TUTORIALS_CPP_PUBLIC
    #define ACTION_TUTORIALS_CPP_LOCAL
  #endif
  #define ACTION_TUTORIALS_CPP_PUBLIC_TYPE
#endif

#ifdef __cplusplus
}
#endif

#endif  // ACTION_TUTORIALS_CPP__VISIBILITY_CONTROL_H_

3.2编写动作服务器

现在，让我们专注于计算Fibonacci数列的动作服务器编写，这会用到前面创建动作课程所创建的动作．

3.2.1编写动作服务器的代码

打开路径并创建action_tutorials_cpp/src/fibonacci_action_server.cpp，然后复制以下代码到里面：

 1#include <functional>
  2#include <memory>
  3#include <thread>
  4
  5#include "action_tutorials_interfaces/action/fibonacci.hpp"
  6#include "rclcpp/rclcpp.hpp"
  7#include "rclcpp_action/rclcpp_action.hpp"
  8#include "rclcpp_components/register_node_macro.hpp"
  9
 10#include "action_tutorials_cpp/visibility_control.h"
 11
 12namespace action_tutorials_cpp
 13{
 14class FibonacciActionServer : public rclcpp::Node
 15{
 16public:
 17  using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
 18  using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
 19
 20  ACTION_TUTORIALS_CPP_PUBLIC
 21  explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
 22  : Node("fibonacci_action_server", options)
 23  {
 24    using namespace std::placeholders;
 25
 26    this->action_server_ = rclcpp_action::create_server<Fibonacci>(
 27      this,
 28      "fibonacci",
 29      std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
 30      std::bind(&FibonacciActionServer::handle_cancel, this, _1),
 31      std::bind(&FibonacciActionServer::handle_accepted, this, _1));
 32  }
 33
 34private:
 35  rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;
 36
 37  rclcpp_action::GoalResponse handle_goal(
 38    const rclcpp_action::GoalUUID & uuid,
 39    std::shared_ptr<const Fibonacci::Goal> goal)
 40  {
 41    RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
 42    (void)uuid;
 43    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
 44  }
 45
 46  rclcpp_action::CancelResponse handle_cancel(
 47    const std::shared_ptr<GoalHandleFibonacci> goal_handle)
 48  {
 49    RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
 50    (void)goal_handle;
 51    return rclcpp_action::CancelResponse::ACCEPT;
 52  }
 53
 54  void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
 55  {
 56    using namespace std::placeholders;
 57    // this needs to return quickly to avoid blocking the executor, so spin up a new thread
 58    std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
 59  }
 60
 61  void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
 62  {
 63    RCLCPP_INFO(this->get_logger(), "Executing goal");
 64    rclcpp::Rate loop_rate(1);
 65    const auto goal = goal_handle->get_goal();
 66    auto feedback = std::make_shared<Fibonacci::Feedback>();
 67    auto & sequence = feedback->partial_sequence;
 68    sequence.push_back(0);
 69    sequence.push_back(1);
 70    auto result = std::make_shared<Fibonacci::Result>();
 71
 72    for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
 73      // Check if there is a cancel request
 74      if (goal_handle->is_canceling()) {
 75        result->sequence = sequence;
 76        goal_handle->canceled(result);
 77        RCLCPP_INFO(this->get_logger(), "Goal canceled");
 78        return;
 79      }
 80      // Update sequence
 81      sequence.push_back(sequence[i] + sequence[i - 1]);
 82      // Publish feedback
 83      goal_handle->publish_feedback(feedback);
 84      RCLCPP_INFO(this->get_logger(), "Publish feedback");
 85
 86      loop_rate.sleep();
 87    }
 88
 89    // Check if goal is done
 90    if (rclcpp::ok()) {
 91      result->sequence = sequence;
 92      goal_handle->succeed(result);
 93      RCLCPP_INFO(this->get_logger(), "Goal succeeded");
 94    }
 95  }
 96};  // class FibonacciActionServer
 97
 98}  // namespace action_tutorials_cpp
 99
100 RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::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(
this,
“fibonacci”,
std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
std::bind(&FibonacciActionServer::handle_cancel, this, _1),
std::bind(&FibonacciActionServer::handle_accepted, this, _1));

一个动作服务器要求有6样东西：

1.模板动作类名：Fibonacci

2.用于添加动作的ros2节点：this

3.动作名称：'fibonacci'

4.负责目标的回调函数：handle_goal

5.负责取消的回调函数：handle_cancel

6.负责目标取消的回调函数：handle_accept

各个回调函数的实现（函数定义）在文件下面．注意，所有的反馈需要返回迅速，否则，有执行器中断风险．

我们看看负责新目标反馈部分：

rclcpp_action::GoalResponse handle_goal(
    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;
  }

这实现的是接收所有目标．

下面回调函数是处理取消（信号）：

  rclcpp_action::CancelResponse handle_cancel(
    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;
  }

这里实现仅是告诉用户，它接受了取消信号．

最后的回调函数接受了一个新目标，并且开始处理它：

  void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
  {
    using namespace std::placeholders;
    // this needs to return quickly to avoid blocking the executor, so spin up a new thread
    std::thread{std::bind(&FibonacciActionServer::execute, this, _1), goal_handle}.detach();
  }

由于execute是一个需要长时间运行的操作,我们开一个线程来做实际的工作,并且可以很快地从handle_accepted返回。

execute方法将要做的处理和更新工作，都是在新线程里面进行的：

 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) {
      // 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()) {
      result->sequence = sequence;
      goal_handle->succeed(result);
      RCLCPP_INFO(this->get_logger(), "Goal succeeded");
    }
  }

工作线程每秒都会处理Fibonacci数列的连串数字，并发布信息反馈每一步的更新．当完成处理过程，它会标记goal_handle为成功，并且退出．

现在我们有了一个完整的功能性动作服务器，编译一下并且运行．

3.2.2编译动作服务器

在前面章节，我们写好了动作服务器代码．为了编译运行它，我们还得做些额外工作．

首先，我们需要配置一下CMakeLists.txt，保证动作服务器可以编译．打开action_tutorials_cpp/CMakeLists.txt，在find_package后面添加下面要调用的内容：

add_library(action_server SHARED
  src/fibonacci_action_server.cpp)
target_include_directories(action_server PRIVATE
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)
target_compile_definitions(action_server
  PRIVATE "ACTION_TUTORIALS_CPP_BUILDING_DLL")
ament_target_dependencies(action_server
  "action_tutorials_interfaces"
  "rclcpp"
  "rclcpp_action"
  "rclcpp_components")
rclcpp_components_register_node(action_server PLUGIN "action_tutorials_cpp::FibonacciActionServer" EXECUTABLE fibonacci_action_server)
install(TARGETS
  action_server
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin)

现在，我们可以编译这个包了．返回action_ws（根据自己实际使用的工作空间修改）工作空间的根目录，执行：

colcon build

3.2.3运行动作服务器

现在我们构建好动作服务器了，可以运行它了．source一下工作空间环境变量，然后尝试运行动作服务器：

ros2 run action_tutorials_cpp fibonacci_action_server

3.3编写动作客户端

3.3.1编写动作客户端代码

打开路径并新建action_tutorials_cpp/src/fibonacci_action_client.cpp，把下面代码放到里面：

  1#include <functional>
  2#include <future>
  3#include <memory>
  4#include <string>
  5#include <sstream>
  6
  7#include "action_tutorials_interfaces/action/fibonacci.hpp"
  8
  9#include "rclcpp/rclcpp.hpp"
 10#include "rclcpp_action/rclcpp_action.hpp"
 11#include "rclcpp_components/register_node_macro.hpp"
 12
 13namespace action_tutorials_cpp
 14{
 15class FibonacciActionClient : public rclcpp::Node
 16{
 17public:
 18  using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
 19  using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;
 20
 21  explicit FibonacciActionClient(const rclcpp::NodeOptions & options)
 22  : Node("fibonacci_action_client", options)
 23  {
 24    this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
 25      this,
 26      "fibonacci");
 27
 28    this->timer_ = this->create_wall_timer(
 29      std::chrono::milliseconds(500),
 30      std::bind(&FibonacciActionClient::send_goal, this));
 31  }
 32
 33  void send_goal()
 34  {
 35    using namespace std::placeholders;
 36
 37    this->timer_->cancel();
 38
 39    if (!this->client_ptr_->wait_for_action_server()) {
 40      RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
 41      rclcpp::shutdown();
 42    }
 43
 44    auto goal_msg = Fibonacci::Goal();
 45    goal_msg.order = 10;
 46
 47    RCLCPP_INFO(this->get_logger(), "Sending goal");
 48
 49    auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
 50    send_goal_options.goal_response_callback =
 51      std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
 52    send_goal_optionsROS2学习笔记20--创建一个action

 ROS2学习笔记14--编写一个简单的服务器和客户端（C++）
 ROS2学习笔记25--ros2话题统计编写教程（C++）
 ROS2学习笔记28--ros2环境下，多激光雷达启动的launch文件编写样式参考
 ROS2学习笔记13--编写一个简单的发布器和侦听器（C++）
 ROS2学习笔记５--认识ros2服务services