ROS2 --- WaitSet（等待集） 等待实体就绪，管理执行回调函数

一、WaitSet的核心定位与底层原理

1.1 WaitSet的作用

ROS2默认的执行器（如SingleThreadedExecutor、MultiThreadedExecutor）会自动管理回调的触发：执行器内部维护一个WaitSet，不断调用wait()等待实体就绪，然后自动执行回调函数。但在以下场景中，默认执行器的"自动性"会成为限制：

• 实时性要求高的场景（如机器人运动控制）：需要手动控制回调执行的优先级和时机；
• 自定义等待逻辑：比如同时等待"话题消息"和"服务请求"，且需按特定顺序处理；
• 嵌入式/资源受限场景：需要精简执行器开销，手动管理等待流程。

WaitSet的本质是ROS2封装的DDS层等待集原语 ，用于监听一组"可等待实体（Waitable）"的状态，当任意实体进入"就绪状态"时，wait()方法返回，开发者可手动处理该实体的逻辑。

1.2 核心定义

• WaitSet ：一个容器，可添加多个Waitable实体，提供wait()方法阻塞等待任意实体就绪，返回就绪实体集合；
• Waitable实体 ：ROS2中可被WaitSet监听的对象，核心包括：
  • Subscription：话题订阅者（就绪条件：有新消息到达且未被读取）；
  • ServiceServer：服务端（就绪条件：有新的服务请求到达）；
  • Client：客户端（就绪条件：服务端返回响应）；
  • Timer：定时器（就绪条件：定时周期到期）；
  • GuardCondition：守护条件（就绪条件：手动调用trigger()触发）；
  • Event：事件对象（如话题匹配状态变化、QoS违规事件）。
• 就绪状态（Ready） ：实体满足"可处理"的条件，且该状态会在wait()返回后被重置（需重新等待下一次就绪）。

1.3 底层DDS关联

ROS2基于DDS（Data Distribution Service）实现通信，WaitSet直接封装了DDS的DDS_WaitSet接口：

• DDS的WaitSet是分布式系统中同步等待数据/事件的标准原语；
• ROS2在DDS WaitSet基础上，封装了面向ROS2实体（如Subscription、Timer）的接口，屏蔽了DDS底层细节；
• 所有Waitable实体最终都会被转换为DDS的Condition对象，添加到DDS WaitSet中等待。

二、WaitSet的C++核心API与使用流程

2.1 核心头文件

使用WaitSet需包含以下头文件（根据使用的实体类型补充）：

#include "rclcpp/wait_set.hpp"          // WaitSet核心头文件
#include "rclcpp/subscription.hpp"      // 订阅者
#include "rclcpp/timer.hpp"             // 定时器
#include "rclcpp/guard_condition.hpp"   // 守护条件
#include "rclcpp/service.hpp"           // 服务端/客户端

2.2 核心方法详解

WaitSet的C++ API围绕"添加实体→等待就绪→处理实体→重置"的流程设计，核心方法如下：

方法	功能	注意事项
add_subscription(Subscription::SharedPtr sub)	添加话题订阅者	重复添加同一实体不会报错，但会冗余监听
add_timer(TimerBase::SharedPtr timer)	添加定时器	定时器需先通过create_timer()创建
add_guard_condition(GuardCondition::SharedPtr gc)	添加守护条件	守护条件需手动触发
add_service(ServiceBase::SharedPtr srv)	添加服务端	仅监听服务请求就绪
add_client(ClientBase::SharedPtr client)	添加客户端	仅监听服务响应就绪
remove_xxx(...)	移除对应实体	移除不存在的实体返回false
clear()	清空所有实体	清空后WaitSet无监听对象
wait(std::chrono::duration<T> timeout)	阻塞等待实体就绪	超时时间可设为rclcpp::Duration::max()（无限等待）
contains(Waitable::SharedPtr entity)	检查实体是否在WaitSet中	可结合ready_set检查是否就绪

关键：wait()的返回值

wait()返回std::pair<rclcpp::WaitResult, rclcpp::ReadySet>：

• WaitResult：枚举类型，标识等待结果：
  • rclcpp::WaitResult::Ready：至少一个实体就绪；
  • rclcpp::WaitResult::Timeout：超时，无实体就绪；
  • rclcpp::WaitResult::Error：等待出错（如实体已销毁）；
• ReadySet：就绪实体的集合，可通过get_subscriptions()、get_timers()等方法获取对应类型的就绪实体。

2.3 完整使用示例（C++）

以下示例实现"同时等待话题消息、定时器、守护条件"，手动处理就绪逻辑，覆盖90%的常用场景：

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "rclcpp/wait_set.hpp"
#include "rclcpp/guard_condition.hpp"

using namespace std::chrono_literals;

int main(int argc, char *argv[]) {
  // 1. 初始化ROS2节点
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("waitset_demo_node");

  // 2. 创建可等待实体
  // 2.1 话题订阅者（无回调，手动取消息）
  auto sub = node->create_subscription<std_msgs::msg::String>(
    "test_topic", 10, [](const std_msgs::msg::String::SharedPtr) {}); // 空回调
  
  // 2.2 定时器（1秒周期）
  auto timer = node->create_wall_timer(
    1s, []() {}); // 空回调，手动触发
  
  // 2.3 守护条件（手动触发）
  auto gc = std::make_shared<rclcpp::GuardCondition>();

  // 3. 初始化WaitSet并添加实体
  rclcpp::WaitSet wait_set;
  wait_set.add_subscription(sub);
  wait_set.add_timer(timer);
  wait_set.add_guard_condition(gc);

  // 4. 循环等待并处理就绪实体
  while (rclcpp::ok()) {
    // 4.1 等待（超时时间2秒，也可设为无限等待：rclcpp::Duration::max()）
    auto [wait_result, ready_set] = wait_set.wait(2s);

    // 4.2 处理等待结果
    if (wait_result == rclcpp::WaitResult::Error) {
      RCLCPP_ERROR(node->get_logger(), "WaitSet等待出错！");
      break;
    } else if (wait_result == rclcpp::WaitResult::Timeout) {
      RCLCPP_INFO(node->get_logger(), "WaitSet超时（2秒），无实体就绪");
      continue;
    }

    // 4.3 处理就绪实体：按优先级（定时器→守护条件→话题）
    // 处理定时器
    if (ready_set.get_timers().count(timer)) {
      RCLCPP_INFO(node->get_logger(), "定时器就绪，执行定时逻辑");
      timer->execute_callback(); // 手动执行定时器回调
      timer->reset(); // 重置定时器（可选，根据需求）
    }

    // 处理守护条件
    if (ready_set.get_guard_conditions().count(gc)) {
      RCLCPP_INFO(node->get_logger(), "守护条件被触发");
      // 守护条件触发后需手动重置（否则下次wait会直接就绪）
      gc->reset();
    }

    // 处理话题订阅者
    if (ready_set.get_subscriptions().count(sub)) {
      std_msgs::msg::String msg;
      rclcpp::MessageInfo msg_info;
      // 手动读取话题消息（核心：替代自动回调）
      if (sub->take(msg, msg_info)) {
        RCLCPP_INFO(node->get_logger(), "收到话题消息：%s", msg.data.c_str());
      }
    }
  }

  // 5. 清理资源
  wait_set.clear();
  rclcpp::shutdown();
  return 0;
}

代码解释：

• 订阅者使用空回调：因为WaitSet模式下手动读取消息，无需自动回调；
• timer->execute_callback()：手动执行定时器回调逻辑，替代Executor的自动执行；
• sub->take()：手动读取就绪的话题消息，这是WaitSet模式下处理订阅者的核心；
• gc->reset()：守护条件触发后需手动重置，否则下次wait()会直接判定为就绪。

2.4 CMakeLists配置（关键）

使用WaitSet需确保链接rclcpp库，示例CMakeLists.txt：

cmake_minimum_required(VERSION 3.8)
project(waitset_demo)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

add_executable(waitset_demo_node src/waitset_demo.cpp)
ament_target_dependencies(waitset_demo_node rclcpp std_msgs)

install(TARGETS
  waitset_demo_node
  DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

三、高级特性与工业级开发注意事项

3.1 线程安全

• WaitSet的所有方法（add_xxx()、wait()、clear()）非线程安全 ，多线程操作时必须加锁：

  std::mutex waitset_mutex;
  // 多线程添加实体时
  std::lock_guard<std::mutex> lock(waitset_mutex);
  wait_set.add_subscription(new_sub);

• 就绪实体的处理逻辑也需保证线程安全（如多线程读取话题消息时加锁）。

3.2 实体生命周期管理

• 若添加到WaitSet的实体（如订阅者、定时器）被销毁（如reset()），调用wait()会返回WaitResult::Error；
• 建议在实体销毁前，先调用remove_xxx()从WaitSet中移除，避免出错。

3.3 实时性优化

• 优先级处理 ：在wait()返回后，可按业务优先级处理就绪实体（如先处理运动控制话题，再处理日志话题）；
• 超时时间调优：实时场景建议设置短超时（如10ms），避免阻塞过久；非实时场景可设为无限等待；
• 避免冗余监听：仅添加需要监听的实体，减少WaitSet的遍历开销。

3.4 QoS与WaitSet的交互

• 对于"可靠传输（Reliable）"的话题：若消息未被确认，WaitSet会持续将订阅者标记为就绪，直到take()读取消息；
• 对于"历史记录（History）"QoS：WaitSet会监听历史消息，需注意take()的调用次数；
• QoS事件（如消息丢失、匹配状态变化）可通过Event实体添加到WaitSet监听。

3.5 与Executor的协同使用

• WaitSet可与Executor共存：比如Executor管理大部分普通回调，WaitSet单独处理实时性要求高的实体；
• 禁止将同一实体同时添加到Executor和WaitSet：会导致回调被重复处理，或实体状态混乱。

四、典型应用场景

1. 机器人实时运动控制：等待"运动指令话题"和"安全停止守护条件"，优先处理安全停止逻辑；
2. 嵌入式机器人开发：精简Executor开销，手动管理WaitSet以降低CPU占用；
3. 多传感器数据同步：等待激光雷达、相机、IMU的话题消息，同步读取并融合数据；
4. 服务/客户端交互：等待服务请求/响应，手动控制超时和重试逻辑。

---

总结

1. 核心定位：WaitSet是ROS2底层的同步等待原语，封装DDS WaitSet，用于手动监听Waitable实体的就绪状态，是Executor的实现基础；
2. 核心流程 ：初始化WaitSet→添加实体→调用wait()等待→检查就绪实体→手动处理（如take()读消息、执行回调）→循环；
3. 关键注意事项：保证线程安全、管理实体生命周期、按业务优先级处理就绪实体，实时场景需调优超时时间。