ROS2入门教程—编写动作服务器和客户端（C++版）

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

1 创建功能包

  开启一个新终端，设置好环境变量，然后进入到dev_ws/src目录下，使用以下命令创建新的功能包action_tutorials_cpp：

ros2 pkg create --dependencies action_tutorials_interfaces rclcpp rclcpp_action rclcpp_components -- action_tutorials_cpp

2 编写动作服务器

  首先，让我们来编写一个动作服务器，该动作服务器使用我们在ROS2入门教程—创建一个动作消息（action）一文中创建的用来计算斐波那契序列的动作。

2.1 编写动作服务器代码

  在action_tutorials_cpp/src目录下创建新文件fibonacci_action_server.cpp，然后拷贝如下代码：

#include <functional>
#include <memory>
#include <thread>

#include "action_tutorials_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"

#include "action_tutorials_cpp/visibility_control.h"
 
namespace action_tutorials_cpp
{
class FibonacciActionServer : public rclcpp::Node
{
public:
    using Fibonacci = action_tutorials_interfaces::action::Fibonacci;
    using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;

    ACTION_TUTORIALS_CPP_PUBLIC
    explicit FibonacciActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
    : Node("fibonacci_action_server", options)
    {
        using namespace std::placeholders;

        this->action_server_ = rclcpp_action::create_server<Fibonacci>(
        this,
        "fibonacci",
        std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
        std::bind(&FibonacciActionServer::handle_cancel, this, _1),
        std::bind(&FibonacciActionServer::handle_accepted, this, _1));
    }
 
private:
    rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;

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

    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");
        }
    }
};  // class FibonacciActionServer
 
}  // namespace action_tutorials_cpp
 
RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionServer)

2.2 代码解释

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

  接下来，我们创建一个类，它是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",
    std::bind(&FibonacciActionServer::handle_goal, this, _1, _2),
    std::bind(&FibonacciActionServer::handle_cancel, this, _1),
    std::bind(&FibonacciActionServer::handle_accepted, this, _1)
);

  一个动作服务器需要以下6个参数：
  $\bullet$ 模板化的动作类型名称：Fibonacci
  $\bullet$ 一个可以将动作服务端添加进来的ROS 2节点：this
  $\bullet$ 动作名称："fibonacci"
  $\bullet$ 一个用于处理目标的回调函数：handle_goal
  $\bullet$ 一个用于处理取消的回调函数：handle_cancel
  $\bullet$ 一个用于处理目标接受的回调函数：handle_accepted

  接下来是三个回调函数的实现，请注意，所有回调函数都需要快速返回，否则可能会堵塞住执行器。
  首先是处理新目标的回调函数：

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

  action是可抢占式的，由于需要执行一段时间，因此开启一个线程来执行实际工作，并且从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");
    }
}

  这个工作线程每秒处理斐波纳契数列的一个序列数，并且发布一个反馈更新。完成处理后，将目标句柄goal_handle标记为成功状态，然后退出。

2.3 编译动作服务器

  首先，需要设置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)

  接下来就可以编译该功能包了，进入到工作空间根目录下，执行下列命令：

colcon build

2.4 运行动作服务器

  打开一个新终端，进入到工作空间根目录，然后运行如下命令：

. install/setup.bash
ros2 run action_tutorials_cpp fibonacci_action_server

3 编写动作客户端

3.1 编写动作客户端代码

  在action_tutorials_cpp/src目录下创建新文件fibonacci_action_client.cpp，然后拷贝如下代码：

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

#include "action_tutorials_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "rclcpp_components/register_node_macro.hpp"

namespace action_tutorials_cpp
{
class FibonacciActionClient : public rclcpp::Node
{
public:
    using Fibonacci = action_tutorials_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"
        );
        this->timer_ = this->create_wall_timer(
            std::chrono::milliseconds(500),
            std::bind(&FibonacciActionClient::send_goal, this)
        );
    }

    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 = std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
        send_goal_options.feedback_callback =std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2);
        send_goal_options.result_callback =std::bind(&FibonacciActionClient::result_callback, this, _1);

        this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
    }

private:
    rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
    rclcpp::TimerBase::SharedPtr timer_;

    void goal_response_callback(std::shared_future<GoalHandleFibonacci::SharedPtr> future)
    {
        auto goal_handle = future.get();
        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");
        }
    }


    void feedback_callback(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());
    }

    void result_callback(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();
    }

};  // class FibonacciActionClient
}  // namespace action_tutorials_cpp

RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionClient)

3.2 代码解释

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

  接下来，我们创建一个类，它是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");

  一个动作客户端需要以下3个参数：
  $\bullet$ 模板化的动作类型名称：Fibonacci
  $\bullet$ 一个可以将动作客户端添加进来的ROS 2节点：this
  $\bullet$ 动作名称："fibonacci"

  接着实例化了一个ROS计时器，用于启动send_goal函数：

this->timer_ = this->create_wall_timer(
    std::chrono::milliseconds(500),
    std::bind(&FibonacciActionClient::send_goal, this)
);

  当计时器到达设定值时，它将调用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 = std::bind(&FibonacciActionClient::goal_response_callback, this, _1);
    send_goal_options.feedback_callback =std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2);
    send_goal_options.result_callback =std::bind(&FibonacciActionClient::result_callback, this, _1);

    this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
}

  该函数会执行以下操作：
  $\bullet$ 取消计时器（这样该函数仅被调用一次）；
  $\bullet$ 等待动作服务器启动；
  $\bullet$ 实例化一个新目标Fibonacci :: Goal；
  $\bullet$ 设置响应、反馈和结果回调
  $\bullet$ 将目标发送到服务器

  当动作服务器接收到目标并接受该目标时，它会向客户端发送响应。该响应由下面的回调函数goal_response_callback处理：

void goal_response_callback(std::shared_future<GoalHandleFibonacci::SharedPtr> future)
{
    auto goal_handle = future.get();
    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");
    }
}

  假如目标已被服务器接受，它就会开始处理。发送给客户端的任何反馈都将由下面的回调函数feedback_callback处理：

void feedback_callback(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());
}

  当服务器完成处理后，会将结果返回给客户端。结果由下面的回调函数result_callback处理：

void result_callback(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();
}

3.3 编译动作客户端

  首先，需要设置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)

  接下来就可以编译该功能包了，进入到工作空间根目录下，执行下列命令：

colcon build

3.4 运行动作客服端

  打开一个新终端，进入到工作空间根目录，然后运行如下命令：

. install/setup.bash
ros2 run action_tutorials_cpp fibonacci_action_client

  运行动作服务器和动作客户端之后，在动作客户端中输出如下信息：

[INFO] [fibonacci_action_client]: Sending goal
[INFO] [fibonacci_action_client]: Goal accepted by server, waiting for result
[INFO] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 
[INFO] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 
[INFO] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5 
[INFO] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5 8 
[INFO] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5 8 13 
[INFO] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5 8 13 21 
[INFO] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5 8 13 21 34 
[INFO] [fibonacci_action_client]: Next number in sequence received: 0 1 1 2 3 5 8 13 21 34 55 
[INFO] [fibonacci_action_client]: Result received: 0 1 1 2 3 5 8 13 21 34 55 

  同时，在动作服务器中输出如下信息：

[INFO] [fibonacci_action_server]: Received goal request with order 10
[INFO] [fibonacci_action_server]: Executing goal
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Publish feedback
[INFO] [fibonacci_action_server]: Goal succeeded