diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp index 5e4c0a218..5d7f2ee49 100644 --- a/rcl/test/rcl/test_graph.cpp +++ b/rcl/test/rcl/test_graph.cpp @@ -1223,175 +1223,75 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_query_functio 9); // number of retries } -/* Test the graph guard condition notices below changes. - * publisher create/destroy, subscription create/destroy - * service create/destroy, client create/destroy - * Other node added/removed +/* Test the graph guard condition notices topic changes. * * Note: this test could be impacted by other communications on the same ROS Domain. */ -TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_condition_trigger_check) { -#define CHECK_GUARD_CONDITION_CHANGE(EXPECTED_RESULT) do { \ - ret = rcl_wait_set_clear(&wait_set); \ - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \ - ret = rcl_wait_set_add_guard_condition(&wait_set, graph_guard_condition, NULL); \ - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; \ - ret = rcl_wait(&wait_set, time_to_sleep.count()); \ - ASSERT_EQ(EXPECTED_RESULT, ret) << rcl_get_error_string().str; \ -} while (0) - +TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_condition_topics) { rcl_ret_t ret; - std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(400); - - rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); - ret = rcl_wait_set_init( - &wait_set, 0, 1, 0, 0, 0, 0, context_ptr, rcl_get_default_allocator()); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - EXPECT_EQ(RCL_RET_OK, rcl_wait_set_fini(&wait_set)) << rcl_get_error_string().str; - }); - + // Create a thread to sleep for a time, then create a publisher, sleep more, then a subscriber, + // sleep more, destroy the subscriber, sleep more, and then destroy the publisher. + std::promise topic_changes_promise; + std::thread topic_thread( + [this, &topic_changes_promise]() { + // sleep + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + // create the publisher + rcl_publisher_t pub = rcl_get_zero_initialized_publisher(); + rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options(); + rcl_ret_t ret = rcl_publisher_init( + &pub, this->node_ptr, ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes), + "/chatter_test_graph_guard_condition_topics", &pub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // sleep + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + // create the subscription + rcl_subscription_t sub = rcl_get_zero_initialized_subscription(); + rcl_subscription_options_t sub_ops = rcl_subscription_get_default_options(); + ret = rcl_subscription_init( + &sub, this->node_ptr, ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes), + "/chatter_test_graph_guard_condition_topics", &sub_ops); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // sleep + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + // destroy the subscription + ret = rcl_subscription_fini(&sub, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // sleep + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + // destroy the publication + ret = rcl_publisher_fini(&pub, this->node_ptr); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + // notify that the thread is done + topic_changes_promise.set_value(true); + }); + // Wait for the graph state to change, expecting it to do so at least 4 times, + // once for each change in the topics thread. const rcl_guard_condition_t * graph_guard_condition = - rcl_node_get_graph_guard_condition(node_ptr); - - // Wait for no graph change condition - int idx = 0; - for (; idx < 100; idx++) { - ret = rcl_wait_set_clear(&wait_set); + rcl_node_get_graph_guard_condition(this->node_ptr); + ASSERT_NE(nullptr, graph_guard_condition) << rcl_get_error_string().str; + std::shared_future future = topic_changes_promise.get_future(); + size_t graph_changes_count = 0; + // while the topic thread is not done, wait and count the graph changes + while (future.wait_for(std::chrono::seconds(0)) != std::future_status::ready) { + ret = rcl_wait_set_clear(this->wait_set_ptr); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_guard_condition(&wait_set, graph_guard_condition, NULL); + ret = rcl_wait_set_add_guard_condition(this->wait_set_ptr, graph_guard_condition, NULL); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait(&wait_set, time_to_sleep.count()); - if (RCL_RET_TIMEOUT == ret) { - break; - } else { - RCUTILS_LOG_INFO_NAMED( - ROS_PACKAGE_NAME, - "waiting for no graph change condition ..."); + std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(400); + RCUTILS_LOG_INFO_NAMED( + ROS_PACKAGE_NAME, + "waiting up to '%s' nanoseconds for graph changes", + std::to_string(time_to_sleep.count()).c_str()); + ret = rcl_wait(this->wait_set_ptr, time_to_sleep.count()); + if (ret == RCL_RET_TIMEOUT) { + continue; } + graph_changes_count++; } - ASSERT_NE(idx, 100); - - // Graph change since creating the publisher - rcl_publisher_t pub = rcl_get_zero_initialized_publisher(); - rcl_publisher_options_t pub_ops = rcl_publisher_get_default_options(); - ret = rcl_publisher_init( - &pub, node_ptr, ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes), - "/chatter_test_graph_guard_condition_topics", &pub_ops); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - - { - SCOPED_TRACE("Check guard condition change failed !"); - CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK); - } - - // Graph change since destroying the publisher - ret = rcl_publisher_fini(&pub, node_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - - { - SCOPED_TRACE("Check guard condition change failed !"); - CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK); - } - - // Graph change since creating the subscription - rcl_subscription_t sub = rcl_get_zero_initialized_subscription(); - rcl_subscription_options_t sub_ops = rcl_subscription_get_default_options(); - ret = rcl_subscription_init( - &sub, node_ptr, ROSIDL_GET_MSG_TYPE_SUPPORT(test_msgs, msg, BasicTypes), - "/chatter_test_graph_guard_condition_topics", &sub_ops); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - - { - SCOPED_TRACE("Check guard condition change failed !"); - CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK); - } - - // Graph change since destroying the subscription - ret = rcl_subscription_fini(&sub, node_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - - { - SCOPED_TRACE("Check guard condition change failed !"); - CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK); - } - - // Graph change since creating service - rcl_service_t service = rcl_get_zero_initialized_service(); - rcl_service_options_t service_options = rcl_service_get_default_options(); - ret = rcl_service_init( - &service, - node_ptr, - ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes), - "test_graph_guard_condition_service", - &service_options); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - - { - SCOPED_TRACE("Check guard condition change failed !"); - CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK); - } - - // Graph change since destroy service - ret = rcl_service_fini(&service, node_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - - { - SCOPED_TRACE("Check guard condition change failed !"); - CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK); - } - - // Graph change since creating client - rcl_client_t client = rcl_get_zero_initialized_client(); - rcl_client_options_t client_options = rcl_client_get_default_options(); - ret = rcl_client_init( - &client, - node_ptr, - ROSIDL_GET_SRV_TYPE_SUPPORT(test_msgs, srv, BasicTypes), - "test_graph_guard_condition_service", - &client_options); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - - { - SCOPED_TRACE("Check guard condition change failed !"); - CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK); - } - - // Graph change since destroying client - ret = rcl_client_fini(&client, node_ptr); - EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - - { - SCOPED_TRACE("Check guard condition change failed !"); - CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK); - } - - // Graph change since adding new node - rcl_node_t node_new = rcl_get_zero_initialized_node(); - rcl_node_options_t node_options = rcl_node_get_default_options(); - ret = rcl_node_init(&node_new, "test_graph2", "", context_ptr, &node_options); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - - { - SCOPED_TRACE("Check guard condition change failed !"); - CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK); - } - - // Graph change since destroying new node - ret = rcl_node_fini(&node_new); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - - { - SCOPED_TRACE("Check guard condition change failed !"); - CHECK_GUARD_CONDITION_CHANGE(RCL_RET_OK); - } - - // Should not get graph change if no change - { - SCOPED_TRACE("Check guard condition change failed !"); - CHECK_GUARD_CONDITION_CHANGE(RCL_RET_TIMEOUT); - } + topic_thread.join(); + // expect at least 4 changes + ASSERT_GE(graph_changes_count, 4ul); } /* Test the rcl_service_server_is_available function.