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

最新推荐文章于 2025-07-25 22:48:02 发布
原创 最新推荐文章于 2025-07-25 22:48:02 发布 · 3.1k 阅读 · 21 ·
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
文章标签：

#其他

本文详细介绍了如何在ROS2中编写动作服务器和客户端，以计算斐波那契序列为例。首先，创建功能包，然后编写动作服务器代码，包括处理目标、取消和接受的目标回调函数。接着，实现执行斐波那契序列的异步工作线程。接着，编写动作客户端，设置目标发送、反馈接收和结果处理的回调。最后，编译并运行服务器和客户端，展示异步通信过程。

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
【ROS】ROS2编程示例：服务和客户端-C++版
郭老二
06-03 965
ROS2入门C++示例：服务端和客户端；包括安装、配置、编辑、编译、运行
C++入门 ros服务通信
tangliang_cn的博客
06-06 472
ros版本noetic参考视频。
【ROS2】初级：客户端-创建和使用插件（C++）
cxyhjl的博客
07-07 1024
目标：学习使用pluginlib创建和加载一个简单的插件。教程级别：初学者时间：20 分钟目录背景先决条件任务创建基类包创建插件包2.1插件的源代码2.2 插件声明 XML2.3 CMake 插件声明使用插件构建并运行摘要背景这个教程源自于 http://wiki.ros.org/pluginlib，以及编写和使用一个简单插件教程。pluginlib是一个 C++库，用于在...
ROS2（c++）
2301_80193801的博客
02-06 1321
概念：概念：节点：ROS中的每个节点应负责单个模块用途（例如，一个节点用于控制车轮电机，一个用于控制激光测距仪等）。每个节点都可以通过主题、服务、操作或参数向其他节点发送和接收数据。工作空间：工作空间可以类比于VS中的SolutionDir，是一个目录，包含build、install、log、src四个子文件夹，src保存源码。功能包：功能包可以理解为存节点的地方，在ROS2中根据编译方式的不同，可以分为：ament_python，适用于python程序cmake，适用于C++
ROS2服务实现（python & C++）
small_jimmy的博客
07-25 1613
在ROS2中，服务（Service）是一种重要的通信机制，它实现了节点之间的同步请求-响应模式。想象一下你在餐厅点餐的场景：你（客户端）向服务员（服务端）提出点餐请求，服务员处理你的请求并给你回复。这就是服务通信的基本模式。服务端（Server）：提供具体服务功能的节点，等待接收请求，处理后返回响应客户端（Client）：需要使用服务的节点，发送请求并等待响应每个服务都需要定义其接口，这就像制定API规范一样。服务接口通过.srv# 请求部分（Request）# 定义客户端向服务端发送的数据格式。
【ROS2】初级：客户端-使用类中的参数（C++）
cxyhjl的博客
07-07 1089
目标：使用 C++创建并运行一个带有 ROS 参数的类。教程级别：初学者时间：20 分钟目录背景先决条件任务1. 创建一个包2. 编写 C++ 节点3. 建立并运行摘要下一步背景在创建您自己的节点时，有时需要添加可以从启动文件设置的参数。这个教程将向您展示如何在 C++类中创建这些参数，以及如何在启动文件中设置它们。先决条件在之前的教程中，您学习了如何创建工作空间和创建软件包。您还...
ROS2动作通信（C++）
孤ROS的博客
07-10 1899
当前工作空间下，启动两个终端，终端1执行动作服务端程序，终端2执行动作客户端程序。最终运行结果与案例类似。
ROS2动作通信(C++实现)
m0_55541357的博客
10-03 1796
感觉每个通信都是那几个流程，觉得困难只要还是因为调用各种类型，各种接口，各种函数太多了，让人乱糟糟的感觉，我认为主要理解了整个流程就行，具体函数还是直接根据模板代码直接修改比较好。
【ROS2】中级-编写动作服务器和客户端（C++）
cxyhjl的博客
07-08 1537
目标：用 C++实现一个动作服务器和客户端。教程级别：中级时间：15 分钟目录背景先决条件任务1. 创建 custom_action_cpp 包2. 编写动作服务器3. 编写动作客户端摘要相关内容背景动作是 ROS 中异步通信的一种形式。动作客户端向动作服务器发送目标请求。动作服务器向动作客户端发送目标反馈和结果。先决条件创建一个动作，您将需要custom_action_int...
ROS与C++入门教程.pdf
07-27
标题《ROS与C++入门教程.pdf》以及描述“介绍利用C++来编写ROS程序，编写ROS参数，ROS服务，ROS发布器，ROS订阅器，分析C++例程，分析roscpp类API”明确表明了该教程的主题集中在如何使用C++语言结合ROS（Robot ...
ROS入门21讲课件&源码.zip
09-26
本资源包“ROS入门21讲课件&源码.zip”提供了学习ROS的初级教程，包括课件和源码，是初学者踏入ROS世界的一份宝贵资料。 1. **ROS基础知识** - **ROS架构**：ROS由节点、消息、服务、参数服务器、图模型等核心概念...
ROS2基础--C++
weixin_44824965的博客
02-15 4424
ROS2
ROS2 + C++ 节点（自用笔记）
weixin_71160458的博客
04-25 3401
自用自用。
ROS2 - C++工程创建和工程解析
Dr_chaser的博客
12-08 1397
本文主要介绍如何创建、编写、编译、运行ROS2的C++工程。开发环境是基于Ubuntu的Linux虚拟机。
ros2与Python入门教程-新建ros2工作空间
ncnynl的专栏
07-03 774
创建ros2的工作空间
《动手学ROS2》5.5Action服务端C++实现
生活不止眼前的苟且
11-12 716
本系列教程作者：小鱼 公众号：鱼香ROS QQ交流群：139707339 教学视频地址：小鱼的B站 完整文档地址：鱼香ROS官网 版权声明：如非允许禁止转载与商业用途。 5.5Action服务端C++实现 因为后续课程使用到Action的地方都是在最后阶段，为了减轻大家学习负担，先不进行讲解，有兴趣的同学可以参考官方例程源码，小鱼这里也贴一份放在这。 #include <inttypes.h> #include <memory> #include "example_interf.
ROS2 + C++ 动作（自用笔记）
weixin_71160458的博客
06-02 2147
自用这里简写了，具体可以参考ROS2 + C++ 自定义接口（自用笔记）根据题目，我们创建一个自定义接口功能包。在src下创建action文件夹，在文件夹下创建文件。#动作自定义接口#目标uint32 write_goal -- - #结果 bool finish -- - #反馈写稿目标写稿完成情况（是or否）写稿数量进度写稿的内容每次编写完代码记得修改和，添加以下代码#添加消息文件和依赖自用。
ROS2与C++入门教程-编写动作客户端
ncnynl的专栏
07-11 522
ROS2与C++入门教程-编写动作客户端
【ROS2】ROS2 Hello World （C++实现）
淋曦的进击手记
11-30 2012
创建工作空间目录（即工程根目录，与ROS1不同，ROS2不需要再初始化工作空间）创建功能包（ROS基本单元，可以理解为一个功能模块，每个工程至少有一个功能包）编辑源文件（写代码实现需求）编辑编译配置文件（ROS2使用ament编译系统，这里编辑CMakeList.txt文件）编译工程（使用colcon编译ROS2工程，生成可执行文件，即ros节点，下文均称作节点）不只是Hello World，所有ROS2工程都是这样的流程。
#include <rclcpp/rclcpp.hpp> #include <nav2_msgs/action/navigate_through_poses.hpp> #include <geometry_msgs/msg/pose_stamped.hpp> #include <rclcpp_action/rclcpp_action.hpp> #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp> #include <cmath> #include <future> // 添加 future 支持 class WaypointNavigator : public rclcpp::Node { public: using NavigateThroughPoses = nav2_msgs::action::NavigateThroughPoses; using GoalHandle = rclcpp_action::ClientGoalHandle<NavigateThroughPoses>; // 构造函数：初始化节点和动作客户端 WaypointNavigator() : Node("wheeltec_waypoint_navigator"), mission_complete_future_(mission_complete_promise_.get_future()) // 初始化 future { // 创建动作客户端 client_ = rclcpp_action::create_client<NavigateThroughPoses>( this, "navigate_through_poses"); // 设置航点 (x, y, 四元数x, y, z, w) std::vector<std::tuple<double, double, double, double, double, double>> waypoints = { {1.040, 0.722, -0.00, 0.00, -0.00, 1.000} }; // 转换为 PoseStamped 数组 poses_.header.frame_id = "map"; poses_.header.stamp = this->now(); for (const auto& [x, y, qx, qy, qz, qw] : waypoints) { geometry_msgs::msg::PoseStamped pose; pose.header.frame_id = "map"; pose.header.stamp = this->now(); pose.pose.position.x = x; pose.pose.position.y = y; pose.pose.position.z = 0.0; // 归一化四元数 double norm = sqrt(qx*qx + qy*qy + qz*qz + qw*qw); if (fabs(norm) < 1e-6) { RCLCPP_ERROR(this->get_logger(), "Invalid quaternion!"); pose.pose.orientation.w = 1.0; // 默认无旋转 } else { pose.pose.orientation.x = qx / norm; pose.pose.orientation.y = qy / norm; pose.pose.orientation.z = qz / norm; pose.pose.orientation.w = qw / norm; } poses_.poses.push_back(pose); } // 等待动作服务器 if (!client_->wait_for_action_server(std::chrono::seconds(5))) { RCLCPP_ERROR(this->get_logger(), "Action server not available"); mission_complete_promise_.set_value(); // 通知主线程退出 return; } sendWaypoints(); } // 获取任务完成的 future std::shared_future<void>& getMissionCompleteFuture() { return mission_complete_future_; } private: rclcpp_action::Client<NavigateThroughPoses>::SharedPtr client_; nav2_msgs::msg::Poses poses_; std::promise<void> mission_complete_promise_; std::shared_future<void> mission_complete_future_; // 发送航点 void sendWaypoints() { auto goal_msg = NavigateThroughPoses::Goal(); goal_msg.poses = poses_; RCLCPP_INFO(this->get_logger(), "Sending %zu waypoints", poses_.poses.size()); auto send_opt = rclcpp_action::Client<NavigateThroughPoses>::SendGoalOptions(); send_opt.goal_response_callback = std::bind(&WaypointNavigator::goalResponse, this, std::placeholders::_1); send_opt.feedback_callback = std::bind(&WaypointNavigator::feedbackCallback, this, std::placeholders::_1, std::placeholders::_2); send_opt.result_callback = std::bind(&WaypointNavigator::resultCallback, this, std::placeholders::_1); client_->async_send_goal(goal_msg, send_opt); } // 目标响应回调 void goalResponse(const GoalHandle::SharedPtr & goal_handle) { if (!goal_handle) { RCLCPP_ERROR(this->get_logger(), "Goal rejected"); mission_complete_promise_.set_value(); // 通知主线程退出 } else { RCLCPP_INFO(this->get_logger(), "Goal accepted"); } } // 反馈回调 void feedbackCallback( GoalHandle::SharedPtr, const std::shared_ptr<const NavigateThroughPoses::Feedback> feedback) { RCLCPP_INFO(this->get_logger(), "Current waypoint: %zu", feedback->current_waypoint + 1); } // 结果回调 void resultCallback(const GoalHandle::WrappedResult & result) { switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: RCLCPP_INFO(this->get_logger(), "All waypoints completed!"); break; case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(this->get_logger(), "Navigation aborted"); break; case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(this->get_logger(), "Navigation canceled"); break; default: RCLCPP_ERROR(this->get_logger(), "Unknown result code"); break; } mission_complete_promise_.set_value(); // 通知主线程任务完成 } }; int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto node = std::make_shared<WaypointNavigator>(); // 等待任务完成或 ROS 关闭 rclcpp::spin_until_future_complete( node, node->getMissionCompleteFuture() ); rclcpp::shutdown(); // 安全关闭 ROS 2 上下文 return 0; }
最新发布
08-06
<think>我们正在分析一个ROS2 C++动作客户端，用于通过航点导航（navigate_through_poses）。用户可能遇到了性能问题或运行时错误，需要优化或故障排除。 根据之前的错误日志，用户遇到了一个关于guard condition创建失败的异常，提示上下文无效（rcl_init未调用或rcl_shutdown已调用）。因此，我们需要检查代码中ROS2上下文的生命周期管理。 常见的优化和故障排除点： 1. 确保正确初始化和关闭ROS2上下文。 2. 动作客户端的生命周期管理（避免在上下文被销毁后使用客户端）。 3. 多线程环境下的安全性。 4. 动作调用的异常处理。 假设用户提供的代码片段（未给出）可能包含以下典型问题： 问题1：未正确初始化或提前关闭上下文 问题2：节点或客户端在作用域结束时被销毁，但未正确处理 问题3：未正确处理动作服务器的响应或超时 下面提供一个优化和增强健壮性的航点导航动作客户端示例代码，并解释关键点。</think>### ROS2 C++动作客户端优化与故障排除 针对`navigate_through_poses`动作客户端的优化和故障排查，结合您的上下文和引用[1]，以下是关键解决方案： --- #### 一、核心问题诊断 1. **上下文生命周期错误**（如您之前遇到的`guard_condition`错误） - **根本原因**：动作客户端可能早于`rclcpp::init()`创建或晚于`rclcpp::shutdown()`调用 - **验证方法**：在创建客户端前添加检查 ```cpp if (!rclcpp::ok()) { RCLCPP_ERROR(logger_, "ROS2 context not initialized!"); return; } ``` 2. **动作服务器连接问题**（引用[1]显示`/bt_navigator`是唯一服务器） - 客户端需匹配服务端的动作类型：`nav2_msgs::action::NavigateThroughPoses` - 使用命令验证服务状态： ```bash ros2 action list -t | grep NavigateThroughPoses # 确认服务存在 ``` --- #### 二、客户端优化策略 1. **异步调用优化**（避免阻塞主线程） ```cpp auto send_goal_options = rclcpp_action::Client<NavigateThroughPoses>::SendGoalOptions(); send_goal_options.goal_response_callback = [](auto) { /* 处理目标响应 */ }; send_goal_options.feedback_callback = [](auto, const auto& feedback) { RCLCPP_INFO(logger_, "剩余航点: %d", feedback->remaining_poses); }; send_goal_options.result_callback = [](const auto& result) { /* 处理结果 */ }; auto future_goal_handle = client_->async_send_goal(goal_msg, send_goal_options); ``` 2. **超时重试机制** ```cpp if (future_goal_handle.wait_for(std::chrono::seconds(5)) != std::future_status::ready) { RCLCPP_WARN(logger_, "动作服务器响应超时，重试中..."); // 重试逻辑 } ``` 3. **资源复用** ```cpp // 单例模式管理客户端 class NavClient { public: static auto getInstance() { static NavClient instance; return &instance; } rclcpp_action::Client<NavigateThroughPoses>::SharedPtr client; private: NavClient() { client = create_client(); } }; ``` --- #### 三、常见故障排查表 | 现象 | 可能原因 | 解决方案 | |------|----------|----------| | `动作服务器未连接` | 1. 节点未启动<br>2. 动作类型不匹配 | 1. 检查`ros2 lifecycle set /bt_navigator activate`<br>2. 确认消息类型为`NavigateThroughPoses` | | `导航中断` | 1. BT节点失败<br>2. 路径规划超时 | 1. 检查`/behavior_tree_log`话题<br>2. 增加`planner_timeout`参数 | | `航点跳过` | 1. 定位漂移<br>2. 容差参数过小 | 1. 检查AMCL/里程计<br>2. 调整`goal_dist_tolerance` | --- #### 四、完整代码优化示例 ```cpp #include "rclcpp_action/rclcpp_action.hpp" #include "nav2_msgs/action/navigate_through_poses.hpp" class WaypointNavigator : public rclcpp::Node { public: using NavigateThroughPoses = nav2_msgs::action::NavigateThroughPoses; WaypointNavigator() : Node("waypoint_client") { client_ = rclcpp_action::create_client<NavigateThroughPoses>(this, "navigate_through_poses"); // 等待服务器（带超时） if (!client_->wait_for_action_server(5s)) { RCLCPP_ERROR(get_logger(), "动作服务器不可用!"); rclcpp::shutdown(); } } void send_waypoints(const std::vector<geometry_msgs::msg::PoseStamped>& poses) { auto goal_msg = NavigateThroughPoses::Goal(); goal_msg.poses = poses; auto send_goal_options = typename rclcpp_action::Client<NavigateThroughPoses>::SendGoalOptions(); send_goal_options.result_callback = [this](const auto& result) { handle_result(result); }; client_->async_send_goal(goal_msg, send_goal_options); } private: void handle_result(const rclcpp_action::ClientGoalHandle<NavigateThroughPoses>::WrappedResult& result) { switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: RCLCPP_INFO(get_logger(), "导航成功!"); break; case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(get_logger(), "导航被中止!"); break; case rclcpp_action::ResultCode::CANCELED: RCLCPP_WARN(get_logger(), "导航已取消"); break; default: RCLCPP_FATAL(get_logger(), "未知错误"); } } rclcpp_action::Client<NavigateThroughPoses>::SharedPtr client_; }; int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = std::make_shared<WaypointNavigator>(); // 示例航点（需替换为实际坐标） std::vector<geometry_msgs::msg::PoseStamped> poses; // ... 填充航点数据 node->send_waypoints(poses); rclcpp::spin(node); rclcpp::shutdown(); return 0; } ``` --- #### 五、调试建议 1. **可视化调试** ```bash ros2 launch nav2_bringup rviz_launch.py # 实时观察导航状态 ``` 2. **行为树监控** ```bash ros2 run nav2_bt_navigator bt_navigator --ros-args -p bt_xml_file:=/path/to/bt.xml ``` 3. **日志分析工具** ```bash ros2 run rqt_console rqt_console # 集中查看所有节点日志 ``` > **关键优化点**：通过`wait_for_action_server()`确保服务器就绪，使用异步回调避免阻塞，结构化处理结果状态码[^1]。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

打赏作者

Roar冷颜

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

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

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

打赏作者

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

抵扣说明：

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

余额充值