failed to call wait_for_message twice for one subscription #1864
Description
Bug report
Required Info:
- Operating System:
- Ubuntu 20.04
- Installation type:
- source
- Version or commit hash:
- DDS implementation:
- Fast-RTPS, RTI Connext, Cyclonedds
- Client library (if applicable):
- rclcpp
Steps to reproduce issue
add the following test case in rclcpp/test/rclcpp/test_wait_for_message.cpp
TEST(TestUtilities, wait_for_message_twice_one_sub) {
rclcpp::init(0, nullptr);
auto node = std::make_shared<rclcpp::Node>("wait_for_message_node");
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>) {});
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);
EXPECT_TRUE(ret);
ret = rclcpp::wait_for_message(out2, sub, node->get_node_options().context(), 5s);
EXPECT_TRUE(ret);
received = true;
});
for (auto i = 0u; i < 20 && received == false; ++i) {
pub->publish(*get_messages_strings()[0]);
std::this_thread::sleep_for(1s);
}
ASSERT_TRUE(received);
EXPECT_EQ(out1, *get_messages_strings()[0]);
EXPECT_EQ(out2, *get_messages_strings()[0]);
rclcpp::shutdown();
}Expected behavior
Test sucessfully
Actual behavior
[ RUN ] TestUtilities.wait_for_message_twice_one_sub
/home/chenlh/Projects/ROS2/ros2-master/src/ros2/rclcpp/rclcpp/test/rclcpp/test_wait_for_message.cpp:102: Failure
Value of: received
Actual: false
Expected: true
[ FAILED ] TestUtilities.wait_for_message_twice_one_sub (20159 ms)
Additional information
I need to add a new test that a subscription with different settings to receive messages from a publisher multiple times, there seems a convenient utility rclcpp::wait_for_message to do it. Unfortunately, it failed as follows,
unknown file: Failure
C++ exception with description "subscription already associated with a wait set" thrown in the test body.
What about creating a PR using the following patch and the above test case?
diff --git a/rclcpp/include/rclcpp/wait_for_message.hpp b/rclcpp/include/rclcpp/wait_for_message.hpp
index 20c9df4d..0f0dc592 100644
--- a/rclcpp/include/rclcpp/wait_for_message.hpp
+++ b/rclcpp/include/rclcpp/wait_for_message.hpp
@@ -56,6 +56,8 @@ bool wait_for_message(
wait_set.add_subscription(subscription);
wait_set.add_guard_condition(gc);
auto ret = wait_set.wait(time_to_wait);
+ wait_set.remove_subscription(subscription);
+ wait_set.remove_guard_condition(gc);
if (ret.kind() != rclcpp::WaitResultKind::Ready) {
return false;
}