diff --git a/rclcpp/include/rclcpp/wait_for_message.hpp b/rclcpp/include/rclcpp/wait_for_message.hpp index 58665ec921..96542e5f4d 100644 --- a/rclcpp/include/rclcpp/wait_for_message.hpp +++ b/rclcpp/include/rclcpp/wait_for_message.hpp @@ -15,19 +15,24 @@ #ifndef RCLCPP__WAIT_FOR_MESSAGE_HPP_ #define RCLCPP__WAIT_FOR_MESSAGE_HPP_ +#include #include #include #include #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 +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 time_to_wait = std::chrono::duration(-1), + const rclcpp::QoS & qos = rclcpp::SystemDefaultsQoS()) +{ + auto sub = rclcpp::create_subscription( + node_parameters, + node_topics, + topic, + qos, + [](const std::shared_ptr) {}); + return wait_for_message( + 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 time_to_wait = std::chrono::duration(-1), const rclcpp::QoS & qos = rclcpp::SystemDefaultsQoS()) { - 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); + out, + node->get_node_parameters_interface(), + node->get_node_topics_interface(), + topic, + time_to_wait, + qos); } } // namespace rclcpp diff --git a/rclcpp/test/rclcpp/test_wait_for_message.cpp b/rclcpp/test/rclcpp/test_wait_for_message.cpp index 3ecd153ef2..546e6907a1 100644 --- a/rclcpp/test/rclcpp/test_wait_for_message.cpp +++ b/rclcpp/test/rclcpp/test_wait_for_message.cpp @@ -15,8 +15,10 @@ #include #include +#include #include #include +#include #include #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(node_name_); + } + + void TearDown() override + { + node_.reset(); + if (rclcpp::ok()) { + rclcpp::shutdown(); + } + } - auto node = std::make_shared("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("wait_for_message_topic", 10); + auto pub = node_->create_publisher(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("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( + topic_name_, 10, [](const std::shared_ptr) {}); 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("wait_for_message_node3"); - +TEST_F(TestWaitForMessage, wait_for_message_twice_one_sub) { using MsgT = test_msgs::msg::Strings; - auto pub = node->create_publisher("wait_for_message_topic", 10); - auto sub = node->create_subscription( - "wait_for_message_topic", 1, [](const std::shared_ptr) {}); + auto pub = node_->create_publisher(topic_name_, 10); + auto sub = node_->create_subscription( + topic_name_, 1, [](const std::shared_ptr) {}); 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,25 +128,20 @@ 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("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("wait_for_last_message_topic", qos); + auto pub = node_->create_publisher(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; }); @@ -132,18 +149,13 @@ TEST(TestUtilities, wait_for_last_message) { 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("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("wait_for_unbounded_uint8_message_topic", qos); + auto pub = node_->create_publisher(topic_name_, qos); MsgT input; input.uint8_values = {1, 2, 3}; input.alignment_check = 42; @@ -153,8 +165,7 @@ 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; }); @@ -162,25 +173,27 @@ TEST(TestUtilities, wait_for_last_message_with_unbounded_uint8_values) { 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(); 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("wait_for_message_custom_context_node", node_opt); + auto node = std::make_shared(node_name, node_opt); using MsgT = test_msgs::msg::Strings; - auto pub = node->create_publisher("wait_for_message_topic", 10); + auto pub = node->create_publisher(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(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]); +} diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index 63cc7e576b..b625194817 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/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) diff --git a/rclcpp_lifecycle/test/test_lifecycle_wait_for_message.cpp b/rclcpp_lifecycle/test/test_lifecycle_wait_for_message.cpp new file mode 100644 index 0000000000..2039a70ecc --- /dev/null +++ b/rclcpp_lifecycle/test/test_lifecycle_wait_for_message.cpp @@ -0,0 +1,89 @@ +// Copyright 2026 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. + +#include + +#include +#include +#include +#include +#include + +#include "rclcpp/wait_for_message.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "test_msgs/msg/strings.hpp" +#include "test_msgs/message_fixtures.hpp" + +using namespace std::chrono_literals; + +class TestLifecycleWaitForMessage : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + const auto * info = ::testing::UnitTest::GetInstance()->current_test_info(); + node_name_ = std::string("wait_for_message_lifecycle_") + info->name(); + topic_name_ = std::string("wait_for_message_lifecycle_topic_") + info->name(); + // unique_ptr: demonstrate shared_from_this is not required. + node_ = std::make_unique(node_name_); + } + + void TearDown() override + { + if (node_) { + node_->shutdown(); + node_.reset(); + } + if (rclcpp::ok()) { + rclcpp::shutdown(); + } + } + + std::string node_name_; + std::string topic_name_; + std::unique_ptr node_; +}; + +TEST_F(TestLifecycleWaitForMessage, wait_for_message_with_lifecycle_node_interfaces) +{ + using MsgT = test_msgs::msg::Strings; + auto pub = node_->create_publisher(topic_name_, 10); + pub->on_activate(); + + MsgT out; + auto received = false; + auto wait = std::async( + [&]() { + // LifecycleNode has no Node::SharedPtr overload; use explicit interfaces. + 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]); +}