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
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

Roar冷颜

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值