failed to call wait_for_message twice for one subscription

[iuhilnehc-ynos]

Bug report

Required Info:

• Operating System:
  • Ubuntu 20.04
• Installation type:
  • source
• Version or commit hash:
  • 80f93d1
• 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;
   }

[fujitatomoya]

@iuhilnehc-ynos

"subscription already associated with a wait set" thrown in the test body.

i do not really understand why this happens. since wait_set is local function scope, why this remove_subscription is required to call? if you have thoughts, could you enlighten me?

rclcpp/rclcpp/include/rclcpp/wait_for_message.hpp

Lines 55 to 57 in 8afef51

	rclcpp::WaitSet wait_set;
	wait_set.add_subscription(subscription);
	wait_set.add_guard_condition(gc);

[iuhilnehc-ynos]

@fujitatomoya

rclcpp/rclcpp/include/rclcpp/wait_for_message.hpp

Line 56 in 8afef51

wait_set.add_subscription(subscription);

call

rclcpp/rclcpp/include/rclcpp/wait_set_template.hpp

Line 151 in 8afef51

local_subscription->exchange_in_use_by_wait_set_state(local_subscription.get(), true);

to exchange in_use_state with true in

rclcpp/rclcpp/src/rclcpp/subscription_base.cpp

Line 280 in 8afef51

return subscription_in_use_by_wait_set_.exchange(in_use_state);

, so using another WaitSet to call wait_set.add_subscription(subscription); for the same subscription without remove_subscription could throw the exception.

rclcpp/rclcpp/include/rclcpp/wait_set_template.hpp

Lines 151 to 154 in 8afef51

	local_subscription->exchange_in_use_by_wait_set_state(local_subscription.get(), true);
	if (already_in_use) {
	throw std::runtime_error("subscription already associated with a wait set");
	}

Notice that the destructor of WaitSet(rclcpp::WaitSetTemplate or rclcpp::wait_set_policies::SequentialSynchronization) does not have the responsibility to call remove_subscription.

[fujitatomoya]

@iuhilnehc-ynos i see it, thanks for the explanation.