wait_for_message.hpp

// Copyright 2021 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#ifndef RCLCPP__WAIT_FOR_MESSAGE_HPP_
#define RCLCPP__WAIT_FOR_MESSAGE_HPP_
 
#include <future>
#include <memory>
#include <string>
 
#include "rcpputils/scope_exit.hpp"
 
#include "rclcpp/node.hpp"
#include "rclcpp/visibility_control.hpp"
#include "rclcpp/wait_set.hpp"
#include "rclcpp/qos.hpp"
 
namespace rclcpp
{
 
template<class MsgT, class Rep = int64_t, class Period = std::milli>
bool wait_for_message(
  MsgT & out,
  std::shared_ptr<rclcpp::Subscription<MsgT>> subscription,
  std::shared_ptr<rclcpp::Context> context,
  std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1))
{
  auto gc = std::make_shared<rclcpp::GuardCondition>(context);
  auto shutdown_callback_handle = context->add_on_shutdown_callback(
    [weak_gc = std::weak_ptr<rclcpp::GuardCondition>{gc}]() {
      auto strong_gc = weak_gc.lock();
      if (strong_gc) {
        strong_gc->trigger();
      }
    });
 
  rclcpp::WaitSet wait_set({}, {}, {}, {}, {}, {}, context);
  wait_set.add_subscription(subscription);
  RCPPUTILS_SCOPE_EXIT(wait_set.remove_subscription(subscription); );
  wait_set.add_guard_condition(gc);
  auto ret = wait_set.wait(time_to_wait);
  if (ret.kind() != rclcpp::WaitResultKind::Ready) {
    return false;
  }
 
  if (wait_set.get_rcl_wait_set().guard_conditions[0]) {
    return false;
  }
 
  rclcpp::MessageInfo info;
  if (!subscription->take(out, info)) {
    return false;
  }
 
  return true;
}
 
 
template<class MsgT, class Rep = int64_t, class Period = std::milli>
bool wait_for_message(
  MsgT & out,
  rclcpp::Node::SharedPtr node,
  const std::string & topic,
  std::chrono::duration<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1),
  const rclcpp::QoS & qos = rclcpp::SystemDefaultsQoS())
{
  auto sub = node->create_subscription<MsgT>(topic, qos, [](const std::shared_ptr<const MsgT>) {});
  return wait_for_message<MsgT, Rep, Period>(
    out, sub, node->get_node_options().context(), time_to_wait);
}
 
}  // namespace rclcpp
 
#endif  // RCLCPP__WAIT_FOR_MESSAGE_HPP_