c++action 编写(ros-humble)

在 ROS 2 Humble 中使用 C++ 实现 Action 时，核心是使用 rclcpp_action 库。为了避免耗时循环阻塞主线程（导致无法处理取消请求或节点心跳），服务端通常在独立的 std::thread 中运行耗时任务。

使用系统自带的example_interfaces/action/Fibonacciaction接口来进行实现

使用ros2 interface show example_interfaces/action/Fibonacci查看后,得到的结果如下,注意其feedback和result的数据是同名的,都叫sequence

bash 复制代码

# Goal
int32 order
---
# Result
int32[] sequence
---
# Feedback
int32[] sequence

前期准备

首先在工作空间创建两个包action_server和action_client

bash 复制代码

ros2 pkg create --build-type ament_cmake --node-name 可执行文件名 包名

两个包的cmakelist和package.xml中都需要添加如下内容

python 复制代码

find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(ament_index_cpp REQUIRED)
find_package(example_interfaces REQUIRED)
//注意连接到可执行文件上用ament_target_dependencies

xml 复制代码

<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>example_interfaces</depend>

server端代码

cpp 复制代码

#include <example_interfaces/action/detail/fibonacci__struct.hpp>
#include <functional>
#include <memory>
#include <rclcpp/node.hpp>
#include <rclcpp/rate.hpp>
#include <rclcpp_action/create_server.hpp>
#include <rclcpp_action/server.hpp>
#include <rclcpp_action/types.hpp>
#include <thread>
#include <vector>

#include "example_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

class FibonacciActionServer : public rclcpp::Node {
public:
  using Fibonacci = example_interfaces::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
  FibonacciActionServer(
      const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
      : Node("fibonacci_action_server", options) {
    this->action_server_ = rclcpp_action::create_server<Fibonacci>(
        this, "fibonacci",
        [this](
            const rclcpp_action::GoalUUID &uuid,
            std::shared_ptr<const example_interfaces::action::Fibonacci::Goal>
                goal) { return this->handle_goal(uuid, goal); },
        [this](std::shared_ptr<GoalHandleFibonacci> goal_handle) {
          return this->handle_cancel(goal_handle);
        },
        [this](std::shared_ptr<GoalHandleFibonacci> goal_handle) {
          return this->handle_accepted(goal_handle);
        });
    RCLCPP_INFO(this->get_logger(), "Fibonacci Action Server 已启动");
  }

private:
  rclcpp_action::Server<example_interfaces::action::Fibonacci>::SharedPtr
      action_server_;

  rclcpp_action::GoalResponse handle_goal(
      const rclcpp_action::GoalUUID &uuid,
      std::shared_ptr<const example_interfaces::action::Fibonacci::Goal> goal) {
    RCLCPP_INFO(this->get_logger(), "收到目标请求 order=%d", goal->order);
    //如果本次请求参数不合规,则返回reject拒绝
    if (goal->order <= 0) {
      RCLCPP_INFO(this->get_logger(), "目标参数order=%d非法，拒绝请求",
                  goal->order);
      return rclcpp_action::GoalResponse::REJECT;
    }
    //参数合规,则返回accept,并且开始执行
    return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
  }
  // 2. 取消请求回调：决定是否允许取消
  rclcpp_action::CancelResponse
  handle_cancel(const std::shared_ptr<GoalHandleFibonacci> goal_handle) {
    RCLCPP_INFO(this->get_logger(), "收到取消请求");
    (void)goal_handle;
    //判断通过,即可以取消action的话则返回accept
    return rclcpp_action::CancelResponse::ACCEPT;
  }
  // 3. 确认接受目标：在此处启动新线程执行耗时任务，避免阻塞主执行器
  void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle) {
    // 启动分离线程执行实际任务,与主线程分离
    std::thread([this, goal_handle]() { this->execute(goal_handle); }).detach();
  }
  // 4. 耗时任务执行函数（运行在独立工作线程中）
  void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle) {
    RCLCPP_INFO(this->get_logger(), "开始执行任务...");

    rclcpp::Rate loop_rate(1.0); // 1hz的定时器对象

    auto goal = goal_handle->get_goal();
    auto feedback = std::make_shared<Fibonacci::Feedback>();
    //这里增加一个对feedback对象中的sequence的引用,修改sequence就是修改feedback->sequence了
    auto &sequence = feedback->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) {
      //检查客户端是否发起了取消
      if (goal_handle->is_canceling()) {
        result->sequence = sequence;
        goal_handle->canceled(result);
      }
      //计算并发布feedback
      sequence.push_back(sequence[i] + sequence[i - 1]);
      goal_handle->publish_feedback(feedback);
      RCLCPP_INFO(this->get_logger(), "发布反馈: %zu 个元素", sequence.size());
      loop_rate.sleep();
    }
    if (rclcpp::ok()) {
      result->sequence = sequence;
      goal_handle->succeed(result);
      RCLCPP_INFO(this->get_logger(), "任务执行完成，成功返回结果");
    }
  }
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<FibonacciActionServer>();
  rclcpp::spin(node);

  rclcpp::shutdown();
  return 0;
}

server端最复杂的地方就是create_server函数需要提供的几个回调函数

handle_goal
- 负责检查goal是否合法,正常的话则会返回accept,并在接下来执行handle_accepted
handle_cancel
- 负责收到取消目标后判断是否取消目标
handle_accepted
- 负责在检查通过后开始执行任务,一般在里面单独开个线程执行(不阻塞主线程,并且还能服务多个client)

client端代码

cpp 复制代码

#include <functional>
#include <future>
#include <memory>
#include <rclcpp/logging.hpp>
#include <rclcpp_action/create_client.hpp>
#include <sstream>
#include <string>

#include "example_interfaces/action/fibonacci.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

class FibonacciActionClient : public rclcpp::Node {
public:
  using Fibonacci = example_interfaces::action::Fibonacci;
  using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;

  explicit FibonacciActionClient(
      const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
      : Node("fibonacci_action_client", options) {
    this->client_ptr_ =
        rclcpp_action::create_client<Fibonacci>(this, "fibonacci");
  }
  //自定义的函数,用于发起任务请求
  void send_goal(int order) {

    if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) {
      RCLCPP_ERROR(this->get_logger(), "等待 Action Server 超时");
      rclcpp::shutdown();
      return;
    }

    auto goal_msg = Fibonacci::Goal();
    goal_msg.order = order;
    RCLCPP_INFO(this->get_logger(), "发送目标: order = %d", order);
    auto send_goal_options =
        rclcpp_action::Client<Fibonacci>::SendGoalOptions();
    this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
    send_goal_options.goal_response_callback =
        [this](const GoalHandleFibonacci::SharedPtr &goal_handle) {
          this->goal_response_callback(goal_handle);
        };

    send_goal_options.feedback_callback =
        [this](GoalHandleFibonacci::SharedPtr goal_handle,
               const std::shared_ptr<const Fibonacci::Feedback> feedback) {
          this->feedback_callback(goal_handle, feedback);
        };
    send_goal_options.result_callback =
        [this](const GoalHandleFibonacci::WrappedResult &result) {
          this->result_callback(result);
        };
    //异步非阻塞调用,通过回调函数处理数据
    this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
  }

private:
  rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;

  // 1.服务端是否接单
  void
  goal_response_callback(const GoalHandleFibonacci::SharedPtr &goal_handle) {
    if (!goal_handle) {
      RCLCPP_ERROR(this->get_logger(), "目标被服务端拒绝");
    } else {
      RCLCPP_INFO(this->get_logger(), "目标已被服务端接收，执行中...");
    }
  }
	// 2. 接收进度反馈
  void
  feedback_callback(GoalHandleFibonacci::SharedPtr goal_handle,
                    const std::shared_ptr<const Fibonacci::Feedback> feedback) {
    std::stringstream ss;
    ss << "收到进度反馈: ";
    for (auto number : feedback->sequence) {
      ss << number << " ";
    }
    RCLCPP_INFO(this->get_logger(), "%s", ss.str().c_str());
  }

  // 3. 接收最终结果
  void result_callback(const GoalHandleFibonacci::WrappedResult &result) {
    switch (result.code) {
    case rclcpp_action::ResultCode::SUCCEEDED:
      RCLCPP_WARN(this->get_logger(), "任务已完成");
      break;
    case rclcpp_action::ResultCode::ABORTED:
      RCLCPP_ERROR(this->get_logger(), "任务被服务端中止");
      return;
    case rclcpp_action::ResultCode::CANCELED:
      RCLCPP_WARN(this->get_logger(), "任务已取消");
      return;
    default:
      RCLCPP_ERROR(this->get_logger(), "未知返回状态");
      return;
    }
    std::stringstream ss;
    ss << "最终计算结果: ";
    for (auto number : result.result->sequence) {
      ss << number << " ";
    }
    RCLCPP_INFO(this->get_logger(), "%s", ss.str().c_str());
  }
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto action_client = std::make_shared<FibonacciActionClient>();

  // 发送计算前 6 位的目标
  action_client->send_goal(6);

  rclcpp::spin(action_client);
  rclcpp::shutdown();
  return 0;
}

client端也类似,主要是需要在发起action请求的时候提供回调函数

goal_response_callback
- 负责判断请求是否被通过
feedback_callback
- 负责处理发回来的反馈数据
result_callback
- 服务结束后的回调函数(处理最终结果等等)

总结

client代码编写中,各种泛型什么的很多,所以知道其关键的几个函数,然后用这个模板,再自己自定义的改就行