.. _program_listing_file_include_rclcpp_wait_for_message.hpp: Program Listing for File wait_for_message.hpp ============================================= |exhale_lsh| :ref:`Return to documentation for file ` (``include/rclcpp/wait_for_message.hpp``) .. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS .. code-block:: cpp // 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 #include #include "rcpputils/scope_exit.hpp" #include "rclcpp/node.hpp" #include "rclcpp/visibility_control.hpp" #include "rclcpp/wait_set.hpp" namespace rclcpp { template bool wait_for_message( MsgT & out, std::shared_ptr> subscription, std::shared_ptr context, std::chrono::duration time_to_wait = std::chrono::duration(-1)) { auto gc = std::make_shared(context); auto shutdown_callback_handle = context->add_on_shutdown_callback( [weak_gc = std::weak_ptr{gc}]() { auto strong_gc = weak_gc.lock(); if (strong_gc) { strong_gc->trigger(); } }); rclcpp::WaitSet wait_set; 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 bool wait_for_message( MsgT & out, rclcpp::Node::SharedPtr node, const std::string & topic, std::chrono::duration time_to_wait = std::chrono::duration(-1)) { auto sub = node->create_subscription(topic, 1, [](const std::shared_ptr) {}); return wait_for_message( out, sub, node->get_node_options().context(), time_to_wait); } } // namespace rclcpp #endif // RCLCPP__WAIT_FOR_MESSAGE_HPP_