Skip to content

[补充] 使用 rclcpp::WaitSet#

什么是 rclcpp::WaitSet#

如 [call Subscription object] 的 take() 方法中所述(./index.md#call-take-method-of-subscription-object),take() 方法是不可逆的.一旦执行了 take() 方法,订阅对象的状态就会发生变化.因为没有针对 take() 方法的撤消作,所以订阅对象无法恢复到之前的状态.你可以在调用 take() 之前使用 rclcpp::WaitSet 来检查订阅队列中传入消息的到达情况. 以下示例代码显示了 wait_set_.wait() 如何告诉你已经收到了一条消息,并且可以通过 take() 获取.

      auto wait_result = wait_set_.wait(std::chrono::milliseconds(0));
      if (wait_result.kind() == rclcpp::WaitResultKind::Ready &&
          wait_result.get_wait_set().get_rcl_wait_set().subscriptions[0]) {
            sub_->take(msg, msg_info);
            RCLCPP_INFO(this->get_logger(), "Catch message");
            RCLCPP_INFO(this->get_logger(), "I heard: [%s]", msg.data.c_str());

单个 rclcpp::WaitSet 对象能够观察多个订阅对象.如果不同主题有多个订阅,您可以检查每个订阅的传入消息的到达情况.自主机器人领域使用的算法需要多个传入消息,例如传感器数据或驱动状态.对多个订阅使用 rclcpp::WaitSet ,他们能够在不接收任何消息的情况下检查所需的消息是否已到达.

      auto wait_result = wait_set_.wait(std::chrono::milliseconds(0));
      bool received_all_messages = false;
      if (wait_result.kind() == rclcpp::WaitResultKind::Ready) {
        for (auto wait_set_subs : wait_result.get_wait_set().get_rcl_wait_set().subscriptions) {
          if (!wait_set_subs) {
            RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for data...");
            return {};
          }
        }
        received_all_mesages = true;
      }

在上面的代码中,除非使用 rclcpp::WaitSet,否则如果不更改订阅对象的状态,就不可能验证所有需要的消息的到达.

编码方式#

本节通过下面的示例代码介绍如何使用 rclcpp::WaitSet 进行编码.

使用 rclcpp::WaitSet 需要以下三个步骤.

1.声明并初始化 WaitSet#

您必须首先实例化基于 rclcpp::WaitSet 的对象. 以下是 main · takam5f2/ros2_subscription_examples_](https://github.com/takam5f2/ros2_subscription_examples/blob/main/waitset_examples/src/timer_listener_triple_async.cpp) 的 [_ros2_subscription_examples/waitset_examples/src/timer_listener_triple_async.cpp 的片段.

rclcpp::WaitSet wait_set_;
注意

有几种类型的类类似于 rclcpp::WaitSet.可以在运行时配置 rclcpp::WaitSet 对象.它不是线程安全的,如 rclcpp::WaitSet_] 的 [_API 规范中所述(https://docs.ros.org/en/ros2_packages/humble/api/rclcpp/generated/typedef_namespacerclcpp_1ad6fb19c154de27e92430309d2da25ac3.html 替代 rclcpp::WaitSet 的线程安全类由 rclcpp 包提供,如下所示.

2.将触发器(Subscription、Timer 等)注册到 WaitSet#

您需要为基于 rclcpp::WaitSet 的对象注册一个触发器. 以下是 main · takam5f2/ros2_subscription_examples_](https://github.com/takam5f2/ros2_subscription_examples/blob/main/waitset_examples/src/timer_listener_triple_async.cpp) 的 [_ros2_subscription_examples/waitset_examples/src/timer_listener_triple_async.cpp 的片段

    subscriptions_array_[0] = create_subscription<std_msgs::msg::String>("chatter", qos, not_executed_callback, subscription_options);
    subscriptions_array_[1] = create_subscription<std_msgs::msg::String>("slower_chatter", qos, not_executed_callback, subscription_options);
    subscriptions_array_[2] = create_subscription<std_msgs::msg::String>("slowest_chatter", qos, not_executed_callback, subscription_options);

    // Add subscription to waitset
    for (auto & subscription : subscriptions_array_) {
      wait_set_.add_subscription(subscription);
    }

在上面的代码中,add_subscription() 方法将创建的订阅注册到 wait_set_ 对象. 基于 rclcpp::WaitSet 的对象基本上处理对象,每个对象都有相应的回调函数.基于 rclcpp::WaitSet 的对象不仅可以观察基于 Subscription 的对象,还可以观察基于 Timer、Service 或 Action 的对象.单个 rclcpp::WaitSet 对象接受不同类型对象的混合. 可在此处找到用于注册计时器触发器的示例代码.

wait_set_.add_timer(much_slower_timer_);

触发器可以在声明和初始化时注册,如 wait_set_topics_and_timer.cpp from the examples 中所述.

3.验证 WaitSet 结果#

从 rclcpp::WaitSet 返回的测试结果的数据结构是嵌套的. 您可以通过以下 2 个步骤找到 WaitSet 结果;

  1. 验证是否已调用任何触发器
  2. 验证是否已触发指定的触发器

对于第 1 步,以下是从 main · takam5f2/ros2_subscription_examples_](https://github.com/takam5f2/ros2_subscription_examples/blob/main/waitset_examples/src/timer_listener_triple_async.cpp) 的 [_ros2_subscription_examples/waitset_examples/src/timer_listener_triple_async.cpp 获取的示例代码.

      auto wait_result = wait_set_.wait(std::chrono::milliseconds(0));
      if (wait_result.kind() == rclcpp::WaitResultKind::Ready) {
        RCLCPP_INFO(this->get_logger(), "wait_set tells that some subscription is ready");
      } else {
        RCLCPP_INFO(this->get_logger(), "wait_set tells that any subscription is not ready and return");
        return;
      }

在上面的代码中,auto wait_result = wait_set_.wait(std::chrono::milliseconds(0)) 测试 wait_set_ 中的触发器是否已被调用.wait()方法的参数是超时持续时间.如果大于 0 毫秒或秒,该方法将等待收到消息,直到超时过期. Ifwait_result.kind() == rclcpp::WaitResultKind::Readyistrue,则表示已调用任何触发器.

对于第 2 步,以下是从 main · takam5f2/ros2_subscription_examples_](https://github.com/takam5f2/ros2_subscription_examples/blob/main/waitset_examples/src/timer_listener_triple_async.cpp) 的 [_ros2_subscription_examples/waitset_examples/src/timer_listener_triple_async.cpp 获取的示例代码.

      for (size_t i = 0; i < subscriptions_num; i++) {
        if (wait_result.get_wait_set().get_rcl_wait_set().subscriptions[i]) {
          std_msgs::msg::String msg;
          rclcpp::MessageInfo msg_info;
          if (subscriptions_array_[i]->take(msg, msg_info)) {
            RCLCPP_INFO(this->get_logger(), "Catch message via subscription[%ld]", i);
            RCLCPP_INFO(this->get_logger(), "I heard: [%s]", msg.data.c_str());

在上面的代码中,wait_result.get_wait_set().get_rcl_wait_set().subscriptions[i] 指示是否已调用每个单独的触发器.结果存储在 subscriptions 数组中.subscriptions 数组中的顺序与触发器的注册顺序相同.