Add wait_for_message overload using node interfaces

rclcpp/include/rclcpp/wait_for_message.hpp

@@ -15,19 +15,24 @@
 #ifndef RCLCPP__WAIT_FOR_MESSAGE_HPP_
 #define RCLCPP__WAIT_FOR_MESSAGE_HPP_
 
+#include <chrono>
 #include <future>
 #include <memory>
 #include <string>
 
 #include "rcpputils/scope_exit.hpp"
 
+#include "rclcpp/create_subscription.hpp"
 #include "rclcpp/node.hpp"
+#include "rclcpp/node_interfaces/node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/node_topics_interface.hpp"
+#include "rclcpp/qos.hpp"
 #include "rclcpp/visibility_control.hpp"
 #include "rclcpp/wait_set.hpp"
-#include "rclcpp/qos.hpp"
 
 namespace rclcpp
 {
+
 /// Wait for the next incoming message.
 /**
  * Given an already initialized subscription,
@@ -77,6 +82,42 @@ bool wait_for_message(
   return true;
 }
 
+/// Wait for the next incoming message using explicit node interfaces.
+/**
+ * Creates a temporary subscription via \ref rclcpp::create_subscription and waits
+ * for the next message (or until timeout / context shutdown).
+ *
+ * This overload does not require an `rclcpp::Node::SharedPtr`, so it works with
+ * `rclcpp_lifecycle::LifecycleNode` and during construction (no `shared_from_this`).
+ *
+ * \param[out] out is the message to be filled when a new message is arriving.
+ * \param[in] node_parameters parameters interface used when creating the subscription.
+ * \param[in] node_topics topics interface used when creating the subscription.
+ * \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.
+ */
+template<class MsgT, class Rep = int64_t, class Period = std::milli>
+bool wait_for_message(
+  MsgT & out,
+  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters,
+  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+  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 = rclcpp::create_subscription<MsgT>(
+    node_parameters,
+    node_topics,
+    topic,
+    qos,
+    [](const std::shared_ptr<const MsgT>) {});
+  return wait_for_message<MsgT, Rep, Period>(
+    out, sub, node_topics->get_node_base_interface()->get_context(), time_to_wait);
+}
+
 /// Wait for the next incoming message.
 /**
  * Wait for the next incoming message to arrive on a specified topic before the specified timeout.
@@ -97,9 +138,13 @@ bool wait_for_message(
   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);
+    out,
+    node->get_node_parameters_interface(),
+    node->get_node_topics_interface(),
+    topic,
+    time_to_wait,
+    qos);
 }
 
 }  // namespace rclcpp

rclcpp/test/rclcpp/test_wait_for_message.cpp

@@ -15,8 +15,10 @@
 #include <gtest/gtest.h>
 
 #include <chrono>
+#include <future>
 #include <memory>
 #include <string>
+#include <thread>
 #include <vector>
 
 #include "rclcpp/node.hpp"
@@ -28,19 +30,41 @@
 
 using namespace std::chrono_literals;
 
-TEST(TestUtilities, wait_for_message) {
-  rclcpp::init(0, nullptr);
+class TestWaitForMessage : public ::testing::Test
+{
+protected:
+  void SetUp() override
+  {
+    rclcpp::init(0, nullptr);
+    const auto * info = ::testing::UnitTest::GetInstance()->current_test_info();
+    // Unique names avoid cross-talk across tests / RMWs that share a domain.
+    node_name_ = std::string("wait_for_message_") + info->name();
+    topic_name_ = std::string("wait_for_message_topic_") + info->name();
+    node_ = std::make_shared<rclcpp::Node>(node_name_);
+  }
+
+  void TearDown() override
+  {
+    node_.reset();
+    if (rclcpp::ok()) {
+      rclcpp::shutdown();
+    }
+  }
 
-  auto node = std::make_shared<rclcpp::Node>("wait_for_message_node");
+  std::string node_name_;
+  std::string topic_name_;
+  rclcpp::Node::SharedPtr node_;
+};
 
+TEST_F(TestWaitForMessage, wait_for_message) {
   using MsgT = test_msgs::msg::Strings;
-  auto pub = node->create_publisher<MsgT>("wait_for_message_topic", 10);
+  auto pub = node_->create_publisher<MsgT>(topic_name_, 10);
 
   MsgT out;
   auto received = false;
   auto wait = std::async(
     [&]() {
-      auto ret = rclcpp::wait_for_message(out, node, "wait_for_message_topic", 5s);
+      auto ret = rclcpp::wait_for_message(out, node_, topic_name_, 5s);
       EXPECT_TRUE(ret);
       received = true;
     });
@@ -49,50 +73,48 @@ TEST(TestUtilities, wait_for_message) {
     pub->publish(*get_messages_strings()[0]);
     std::this_thread::sleep_for(1s);
   }
+  ASSERT_NO_THROW(wait.get());
   ASSERT_TRUE(received);
   EXPECT_EQ(out, *get_messages_strings()[0]);
-
-  rclcpp::shutdown();
 }
 
-TEST(TestUtilities, wait_for_message_indefinitely) {
-  rclcpp::init(0, nullptr);
-
-  auto node = std::make_shared<rclcpp::Node>("wait_for_message_node2");
-
+TEST_F(TestWaitForMessage, wait_for_message_indefinitely) {
   using MsgT = test_msgs::msg::Strings;
   MsgT out;
   auto received = false;
+  // Create the subscription while the context is still valid. Some RMWs (e.g. zenoh)
+  // throw if wait_for_message tries to create a subscription after shutdown.
+  auto sub = node_->create_subscription<MsgT>(
+    topic_name_, 10, [](const std::shared_ptr<const MsgT>) {});
   auto wait = std::async(
     [&]() {
-      auto ret = rclcpp::wait_for_message(out, node, "wait_for_message_topic" /*, -1 */);
-      EXPECT_TRUE(ret);
-      received = true;
+      auto ret = rclcpp::wait_for_message(out, sub, node_->get_node_options().context());
+      EXPECT_FALSE(ret);
+      received = ret;
     });
 
+  // Let the waiter enter wait_set.wait before interrupting via shutdown.
+  std::this_thread::sleep_for(100ms);
   rclcpp::shutdown();
 
+  ASSERT_NO_THROW(wait.get());
   ASSERT_FALSE(received);
 }
 
-TEST(TestUtilities, wait_for_message_twice_one_sub) {
-  rclcpp::init(0, nullptr);
-
-  auto node = std::make_shared<rclcpp::Node>("wait_for_message_node3");
-
+TEST_F(TestWaitForMessage, wait_for_message_twice_one_sub) {
   using MsgT = test_msgs::msg::Strings;
-  auto pub = node->create_publisher<MsgT>("wait_for_message_topic", 10);
-  auto sub = node->create_subscription<MsgT>(
-    "wait_for_message_topic", 1, [](const std::shared_ptr<const MsgT>) {});
+  auto pub = node_->create_publisher<MsgT>(topic_name_, 10);
+  auto sub = node_->create_subscription<MsgT>(
+    topic_name_, 1, [](const std::shared_ptr<const MsgT>) {});
 
   MsgT out1;
   MsgT out2;
   auto received = false;
   auto wait = std::async(
     [&]() {
-      auto ret = rclcpp::wait_for_message(out1, sub, node->get_node_options().context(), 5s);
+      auto ret = rclcpp::wait_for_message(out1, sub, node_->get_node_options().context(), 5s);
       EXPECT_TRUE(ret);
-      ret = rclcpp::wait_for_message(out2, sub, node->get_node_options().context(), 5s);
+      ret = rclcpp::wait_for_message(out2, sub, node_->get_node_options().context(), 5s);
       EXPECT_TRUE(ret);
       received = true;
     });
@@ -106,44 +128,34 @@ TEST(TestUtilities, wait_for_message_twice_one_sub) {
   ASSERT_TRUE(received);
   EXPECT_EQ(out1, *get_messages_strings()[0]);
   EXPECT_EQ(out2, *get_messages_strings()[0]);
-
-  rclcpp::shutdown();
 }
 
-TEST(TestUtilities, wait_for_last_message) {
-  rclcpp::init(0, nullptr);
-
-  auto node = std::make_shared<rclcpp::Node>("wait_for_last_message_node");
+TEST_F(TestWaitForMessage, wait_for_last_message) {
   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);
+  auto pub = node_->create_publisher<MsgT>(topic_name_, 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);
+      auto ret = rclcpp::wait_for_message(out, node_, topic_name_, 5s, qos);
       EXPECT_TRUE(ret);
       received = true;
     });
 
   ASSERT_NO_THROW(wait.get());
   ASSERT_TRUE(received);
   EXPECT_EQ(out, *get_messages_strings()[0]);
-
-  rclcpp::shutdown();
 }
 
-TEST(TestUtilities, wait_for_last_message_with_unbounded_uint8_values) {
-  rclcpp::init(0, nullptr);
-
-  auto node = std::make_shared<rclcpp::Node>("wait_for_unbounded_uint8_message_node");
+TEST_F(TestWaitForMessage, wait_for_last_message_with_unbounded_uint8_values) {
   auto qos = rclcpp::QoS(1).reliable().transient_local();
 
   using MsgT = test_msgs::msg::UnboundedSequences;
-  auto pub = node->create_publisher<MsgT>("wait_for_unbounded_uint8_message_topic", qos);
+  auto pub = node_->create_publisher<MsgT>(topic_name_, qos);
   MsgT input;
   input.uint8_values = {1, 2, 3};
   input.alignment_check = 42;
@@ -153,34 +165,35 @@ TEST(TestUtilities, wait_for_last_message_with_unbounded_uint8_values) {
   auto received = false;
   auto wait = std::async(
     [&]() {
-      auto ret = rclcpp::wait_for_message(
-        out, node, "wait_for_unbounded_uint8_message_topic", 5s, qos);
+      auto ret = rclcpp::wait_for_message(out, node_, topic_name_, 5s, qos);
       EXPECT_TRUE(ret);
       received = true;
     });
 
   ASSERT_NO_THROW(wait.get());
   ASSERT_TRUE(received);
   EXPECT_EQ(out, input);
-
-  rclcpp::shutdown();
 }
 
-TEST(TestUtilities, wait_for_message_custom_context) {
+TEST(TestWaitForMessageCustomContext, wait_for_message_custom_context) {
   auto context = std::make_shared<rclcpp::Context>();
   context->init(0, nullptr);
 
+  const auto * info = ::testing::UnitTest::GetInstance()->current_test_info();
+  const std::string node_name = std::string("wait_for_message_custom_context_") + info->name();
+  const std::string topic_name = std::string("wait_for_message_topic_") + info->name();
+
   auto node_opt = rclcpp::NodeOptions().context(context);
-  auto node = std::make_shared<rclcpp::Node>("wait_for_message_custom_context_node", node_opt);
+  auto node = std::make_shared<rclcpp::Node>(node_name, node_opt);
 
   using MsgT = test_msgs::msg::Strings;
-  auto pub = node->create_publisher<MsgT>("wait_for_message_topic", 10);
+  auto pub = node->create_publisher<MsgT>(topic_name, 10);
 
   MsgT out;
   auto received = false;
   auto wait = std::async(
     [&]() {
-      auto ret = rclcpp::wait_for_message(out, node, "wait_for_message_topic", 5s);
+      auto ret = rclcpp::wait_for_message(out, node, topic_name, 5s);
       EXPECT_TRUE(ret);
       received = true;
     });
@@ -189,8 +202,37 @@ TEST(TestUtilities, wait_for_message_custom_context) {
     pub->publish(*get_messages_strings()[0]);
     std::this_thread::sleep_for(1s);
   }
+  ASSERT_NO_THROW(wait.get());
   ASSERT_TRUE(received);
   EXPECT_EQ(out, *get_messages_strings()[0]);
 
+  node.reset();
   context->shutdown("test complete");
 }
+
+TEST_F(TestWaitForMessage, wait_for_message_explicit_interfaces) {
+  using MsgT = test_msgs::msg::Strings;
+  auto pub = node_->create_publisher<MsgT>(topic_name_, 10);
+
+  MsgT out;
+  auto received = false;
+  auto wait = std::async(
+    [&]() {
+      auto ret = rclcpp::wait_for_message(
+        out,
+        node_->get_node_parameters_interface(),
+        node_->get_node_topics_interface(),
+        topic_name_,
+        5s);
+      EXPECT_TRUE(ret);
+      received = true;
+    });
+
+  for (auto i = 0u; i < 10 && received == false; ++i) {
+    pub->publish(*get_messages_strings()[0]);
+    std::this_thread::sleep_for(1s);
+  }
+  ASSERT_NO_THROW(wait.get());
+  ASSERT_TRUE(received);
+  EXPECT_EQ(out, *get_messages_strings()[0]);
+}

rclcpp_lifecycle/CMakeLists.txt

@@ -119,6 +119,17 @@ if(BUILD_TESTING)
       rclcpp::rclcpp
       test_msgs::test_msgs)
   endif()
+  ament_add_ros_isolated_gtest(
+    test_lifecycle_wait_for_message
+    test/test_lifecycle_wait_for_message.cpp
+    TIMEOUT 120)
+  if(TARGET test_lifecycle_wait_for_message)
+    target_link_libraries(
+      test_lifecycle_wait_for_message
+      ${PROJECT_NAME}
+      rclcpp::rclcpp
+      test_msgs::test_msgs)
+  endif()
   ament_add_gtest(test_lifecycle_service_client test/test_lifecycle_service_client.cpp TIMEOUT 120)
   ament_add_test_label(test_lifecycle_service_client mimick)
   if(TARGET test_lifecycle_service_client)