Add qos parameter for wait_for_message function (#2903) (#2906)

(cherry picked from commit 2fcef70)

Signed-off-by: Sriharsha Ghanta <[email protected]>
Signed-off-by: Alejandro Hernandez Cordero <[email protected]>
Co-authored-by: Sriharsha Ghanta <[email protected]>
Co-authored-by: Alejandro Hernandez Cordero <[email protected]>

Author: 3 people

rclcpp/include/rclcpp/wait_for_message.hpp

@@ -15,6 +15,7 @@
 #ifndef RCLCPP__WAIT_FOR_MESSAGE_HPP_
 #define RCLCPP__WAIT_FOR_MESSAGE_HPP_
 
+#include <future>
 #include <memory>
 #include <string>
 
@@ -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<Rep, Period> time_to_wait = std::chrono::duration<Rep, Period>(-1))
+  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, 1, [](const std::shared_ptr<const MsgT>) {});
+  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);
 }

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<rclcpp::Node>("wait_for_last_message_node");
+  auto qos = rclcpp::QoS(1).reliable().transient_local();
+
+  using MsgT = test_msgs::msg::Strings;
+  auto pub = node->create_publisher<MsgT>("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();
+}