diff --git a/rclcpp/include/rclcpp/wait_for_message.hpp b/rclcpp/include/rclcpp/wait_for_message.hpp index 25c45ad782..fab2e6ccfc 100644 --- a/rclcpp/include/rclcpp/wait_for_message.hpp +++ b/rclcpp/include/rclcpp/wait_for_message.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__WAIT_FOR_MESSAGE_HPP_ #define RCLCPP__WAIT_FOR_MESSAGE_HPP_ +#include #include #include @@ -23,6 +24,7 @@ #include "rclcpp/node.hpp" #include "rclcpp/visibility_control.hpp" #include "rclcpp/wait_set.hpp" +#include "rclcpp/qos.hpp" namespace rclcpp { @@ -79,10 +81,11 @@ bool wait_for_message( /** * Wait for the next incoming message to arrive on a specified topic before the specified timeout. * - * \param[out] out is the message to be filled when a new message is arriving. + * \param[out] out is the message to be filled when a new message is arriving * \param[in] node the node pointer to initialize the subscription on. * \param[in] topic the topic to wait for messages. * \param[in] time_to_wait parameter specifying the timeout before returning. + * \param[in] qos parameter specifying QoS settings for the subscription. * \return true if a message was successfully received, false if message could not * be obtained or shutdown was triggered asynchronously on the context. */ @@ -91,9 +94,10 @@ bool wait_for_message( MsgT & out, rclcpp::Node::SharedPtr node, const std::string & topic, - std::chrono::duration time_to_wait = std::chrono::duration(-1)) + std::chrono::duration time_to_wait = std::chrono::duration(-1), + const rclcpp::QoS & qos = rclcpp::SystemDefaultsQoS()) { - auto sub = node->create_subscription(topic, 1, [](const std::shared_ptr) {}); + auto sub = node->create_subscription(topic, qos, [](const std::shared_ptr) {}); return wait_for_message( out, sub, node->get_node_options().context(), time_to_wait); } diff --git a/rclcpp/test/rclcpp/test_wait_for_message.cpp b/rclcpp/test/rclcpp/test_wait_for_message.cpp index 60dd6e4c05..9f49fb141c 100644 --- a/rclcpp/test/rclcpp/test_wait_for_message.cpp +++ b/rclcpp/test/rclcpp/test_wait_for_message.cpp @@ -108,3 +108,29 @@ TEST(TestUtilities, wait_for_message_twice_one_sub) { rclcpp::shutdown(); } + +TEST(TestUtilities, wait_for_last_message) { + rclcpp::init(0, nullptr); + + auto node = std::make_shared("wait_for_last_message_node"); + auto qos = rclcpp::QoS(1).reliable().transient_local(); + + using MsgT = test_msgs::msg::Strings; + auto pub = node->create_publisher("wait_for_last_message_topic", qos); + pub->publish(*get_messages_strings()[0]); + + MsgT out; + auto received = false; + auto wait = std::async( + [&]() { + auto ret = rclcpp::wait_for_message(out, node, "wait_for_last_message_topic", 5s, qos); + EXPECT_TRUE(ret); + received = true; + }); + + ASSERT_NO_THROW(wait.get()); + ASSERT_TRUE(received); + EXPECT_EQ(out, *get_messages_strings()[0]); + + rclcpp::shutdown(); +}