diff --git a/rcl/test/CMakeLists.txt b/rcl/test/CMakeLists.txt index 261f8bd82..94abddd40 100644 --- a/rcl/test/CMakeLists.txt +++ b/rcl/test/CMakeLists.txt @@ -191,7 +191,7 @@ function(test_target_function) ) rcl_add_custom_gtest(test_service${target_suffix} - SRCS rcl/test_service.cpp + SRCS rcl/test_service.cpp rcl/common.cpp ENV ${rmw_implementation_env_var} APPEND_LIBRARY_DIRS ${extra_lib_dirs} LIBRARIES ${PROJECT_NAME} @@ -199,7 +199,7 @@ function(test_target_function) ) rcl_add_custom_gtest(test_subscription${target_suffix} - SRCS rcl/test_subscription.cpp + SRCS rcl/test_subscription.cpp rcl/common.cpp ENV ${rmw_implementation_env_var} APPEND_LIBRARY_DIRS ${extra_lib_dirs} LIBRARIES ${PROJECT_NAME} @@ -259,13 +259,13 @@ function(test_target_function) # Launch tests rcl_add_custom_executable(service_fixture${target_suffix} - SRCS rcl/service_fixture.cpp + SRCS rcl/service_fixture.cpp rcl/common.cpp LIBRARIES ${PROJECT_NAME} AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" ) rcl_add_custom_executable(client_fixture${target_suffix} - SRCS rcl/client_fixture.cpp + SRCS rcl/client_fixture.cpp rcl/common.cpp LIBRARIES ${PROJECT_NAME} AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs" ) diff --git a/rcl/test/rcl/client_fixture.cpp b/rcl/test/rcl/client_fixture.cpp index 650b14bf4..4c2eadfd1 100644 --- a/rcl/test/rcl/client_fixture.cpp +++ b/rcl/test/rcl/client_fixture.cpp @@ -12,11 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include - #include "rcutils/logging_macros.h" #include "rcl/client.h" @@ -26,87 +21,7 @@ #include "osrf_testing_tools_cpp/scope_exit.hpp" #include "rcl/error_handling.h" -#include "rcl/graph.h" - -bool -wait_for_server_to_be_available( - rcl_node_t * node, - rcl_client_t * client, - size_t max_tries, - int64_t period_ms) -{ - size_t iteration = 0; - do { - ++iteration; - bool is_ready; - rcl_ret_t ret = rcl_service_server_is_available(node, client, &is_ready); - if (ret != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, - "Error in rcl_service_server_is_available: %s", - rcl_get_error_string().str); - return false; - } - if (is_ready) { - return true; - } - std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); - } while (iteration < max_tries); - return false; -} - -bool -wait_for_client_to_be_ready( - rcl_client_t * client, - rcl_context_t * context, - size_t max_tries, - int64_t period_ms) -{ - rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); - rcl_ret_t ret = - rcl_wait_set_init(&wait_set, 0, 0, 0, 1, 0, 0, context, rcl_get_default_allocator()); - if (ret != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string().str); - return false; - } - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string().str); - throw std::runtime_error("error while waiting for client"); - } - }); - size_t iteration = 0; - do { - ++iteration; - if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string().str); - return false; - } - if (rcl_wait_set_add_client(&wait_set, client, NULL) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in wait_set_add_client: %s", rcl_get_error_string().str); - return false; - } - ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); - if (ret == RCL_RET_TIMEOUT) { - continue; - } - if (ret != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Error in wait: %s", rcl_get_error_string().str); - return false; - } - for (size_t i = 0; i < wait_set.size_of_clients; ++i) { - if (wait_set.clients[i] && wait_set.clients[i] == client) { - return true; - } - } - } while (iteration < max_tries); - return false; -} +#include "common.hpp" int main(int argc, char ** argv) { diff --git a/rcl/test/rcl/common.cpp b/rcl/test/rcl/common.cpp new file mode 100644 index 000000000..a28650690 --- /dev/null +++ b/rcl/test/rcl/common.cpp @@ -0,0 +1,237 @@ +// Copyright 2020 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 "common.hpp" + +#include +#include + +#include "rcutils/logging_macros.h" + +#include "osrf_testing_tools_cpp/scope_exit.hpp" +#include "rcl/error_handling.h" +#include "rcl/graph.h" + +bool +wait_for_server_to_be_available( + rcl_node_t * node, + rcl_client_t * client, + size_t max_tries, + int64_t period_ms) +{ + size_t iteration = 0; + while (iteration < max_tries) { + ++iteration; + bool is_ready; + rcl_ret_t ret = rcl_service_server_is_available(node, client, &is_ready); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "Error in rcl_service_server_is_available: %s", + rcl_get_error_string().str); + return false; + } + if (is_ready) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); + } + return false; +} + +bool +wait_for_client_to_be_ready( + rcl_client_t * client, + rcl_context_t * context, + size_t max_tries, + int64_t period_ms) +{ + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 0, 0, 0, 1, 0, 0, context, rcl_get_default_allocator()); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string().str); + return false; + } + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string().str); + throw std::runtime_error("error while waiting for client"); + } + }); + size_t iteration = 0; + while (iteration < max_tries) { + ++iteration; + if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string().str); + return false; + } + if (rcl_wait_set_add_client(&wait_set, client, NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait_set_add_client: %s", rcl_get_error_string().str); + return false; + } + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); + if (ret == RCL_RET_TIMEOUT) { + continue; + } + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Error in wait: %s", rcl_get_error_string().str); + return false; + } + for (size_t i = 0; i < wait_set.size_of_clients; ++i) { + if (wait_set.clients[i] && wait_set.clients[i] == client) { + return true; + } + } + } + return false; +} + +bool +wait_for_service_to_be_ready( + rcl_service_t * service, + rcl_context_t * context, + size_t max_tries, + int64_t period_ms) +{ + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, 0, context, rcl_get_default_allocator()); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string().str); + return false; + } + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string().str); + throw std::runtime_error("error waiting for service to be ready"); + } + }); + size_t iteration = 0; + while (iteration < max_tries) { + ++iteration; + if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string().str); + return false; + } + if (rcl_wait_set_add_service(&wait_set, service, NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait_set_add_service: %s", rcl_get_error_string().str); + return false; + } + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); + if (ret == RCL_RET_TIMEOUT) { + continue; + } + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Error in wait: %s", rcl_get_error_string().str); + return false; + } + for (size_t i = 0; i < wait_set.size_of_services; ++i) { + if (wait_set.services[i] && wait_set.services[i] == service) { + return true; + } + } + } + return false; +} + +bool +wait_for_established_subscription( + const rcl_publisher_t * publisher, + size_t max_tries, + int64_t period_ms) +{ + size_t iteration = 0; + rcl_ret_t ret = RCL_RET_OK; + size_t subscription_count = 0; + while (iteration < max_tries) { + ++iteration; + ret = rcl_publisher_get_subscription_count(publisher, &subscription_count); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, + "Error in rcl_publisher_get_subscription_count: %s", + rcl_get_error_string().str); + return false; + } + if (subscription_count == 1) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); + } + return false; +} + +bool +wait_for_subscription_to_be_ready( + rcl_subscription_t * subscription, + rcl_context_t * context, + size_t max_tries, + int64_t period_ms) +{ + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = + rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, context, rcl_get_default_allocator()); + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string().str); + return false; + } + OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( + { + if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string().str); + throw std::runtime_error("error waiting for service to be ready"); + } + }); + size_t iteration = 0; + while (iteration < max_tries) { + ++iteration; + if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string().str); + return false; + } + if (rcl_wait_set_add_subscription(&wait_set, subscription, NULL) != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED( + ROS_PACKAGE_NAME, "Error in rcl_wait_set_add_subscription: %s", rcl_get_error_string().str); + return false; + } + ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); + if (ret == RCL_RET_TIMEOUT) { + continue; + } + if (ret != RCL_RET_OK) { + RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Error in wait: %s", rcl_get_error_string().str); + return false; + } + for (size_t i = 0; i < wait_set.size_of_subscriptions; ++i) { + if (wait_set.subscriptions[i] && wait_set.subscriptions[i] == subscription) { + return true; + } + } + } + return false; +} diff --git a/rcl/test/rcl/common.hpp b/rcl/test/rcl/common.hpp new file mode 100644 index 000000000..efdaf6322 --- /dev/null +++ b/rcl/test/rcl/common.hpp @@ -0,0 +1,56 @@ +// Copyright 2020 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. + +#ifndef RCL__COMMON_HPP_ +#define RCL__COMMON_HPP_ + +#include "rcl/client.h" +#include "rcl/service.h" +#include "rcl/rcl.h" + +bool +wait_for_server_to_be_available( + rcl_node_t * node, + rcl_client_t * client, + size_t max_tries, + int64_t period_ms); + +bool +wait_for_client_to_be_ready( + rcl_client_t * client, + rcl_context_t * context, + size_t max_tries, + int64_t period_ms); + +bool +wait_for_service_to_be_ready( + rcl_service_t * service, + rcl_context_t * context, + size_t max_tries, + int64_t period_ms); + +bool +wait_for_established_subscription( + const rcl_publisher_t * publisher, + size_t max_tries, + int64_t period_ms); + +bool +wait_for_subscription_to_be_ready( + rcl_subscription_t * subscription, + rcl_context_t * context, + size_t max_tries, + int64_t period_ms); + +#endif // RCL__COMMON_HPP_ diff --git a/rcl/test/rcl/service_fixture.cpp b/rcl/test/rcl/service_fixture.cpp index 74a1d2795..acad4504f 100644 --- a/rcl/test/rcl/service_fixture.cpp +++ b/rcl/test/rcl/service_fixture.cpp @@ -13,8 +13,6 @@ // limitations under the License. #include -#include -#include #include #include "rcutils/logging_macros.h" @@ -27,59 +25,8 @@ #include "osrf_testing_tools_cpp/scope_exit.hpp" #include "rcl/error_handling.h" +#include "common.hpp" -bool -wait_for_service_to_be_ready( - rcl_service_t * service, - rcl_context_t * context, - size_t max_tries, - int64_t period_ms) -{ - rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); - rcl_ret_t ret = - rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, 0, context, rcl_get_default_allocator()); - if (ret != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string().str); - return false; - } - OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT( - { - if (rcl_wait_set_fini(&wait_set) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in wait set fini: %s", rcl_get_error_string().str); - throw std::runtime_error("error waiting for service to be ready"); - } - }); - size_t iteration = 0; - do { - ++iteration; - if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string().str); - return false; - } - if (rcl_wait_set_add_service(&wait_set, service, NULL) != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in wait_set_add_service: %s", rcl_get_error_string().str); - return false; - } - ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); - if (ret == RCL_RET_TIMEOUT) { - continue; - } - if (ret != RCL_RET_OK) { - RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Error in wait: %s", rcl_get_error_string().str); - return false; - } - for (size_t i = 0; i < wait_set.size_of_services; ++i) { - if (wait_set.services[i] && wait_set.services[i] == service) { - return true; - } - } - } while (iteration < max_tries); - return false; -} int main(int argc, char ** argv) { diff --git a/rcl/test/rcl/test_service.cpp b/rcl/test/rcl/test_service.cpp index 33bce02a0..59760e70c 100644 --- a/rcl/test/rcl/test_service.cpp +++ b/rcl/test/rcl/test_service.cpp @@ -14,19 +14,14 @@ #include -#include -#include -#include - -#include "rcl/graph.h" #include "rcl/service.h" - #include "rcl/rcl.h" #include "test_msgs/srv/basic_types.h" #include "osrf_testing_tools_cpp/scope_exit.hpp" #include "rcl/error_handling.h" +#include "common.hpp" #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX @@ -77,79 +72,6 @@ class CLASSNAME (TestServiceFixture, RMW_IMPLEMENTATION) : public ::testing::Tes } }; -rcl_ret_t -wait_for_server_to_be_available( - rcl_node_t * node, - rcl_client_t * client, - size_t max_tries, - int64_t period_ms, - bool & success) -{ - size_t iteration = 0; - bool is_ready = false; - rcl_ret_t ret = RCL_RET_OK; - while (iteration < max_tries) { - ++iteration; - ret = rcl_service_server_is_available(node, client, &is_ready); - if (ret != RCL_RET_OK) { - break; - } - if (is_ready) { - success = true; - return ret; - } - std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); - } - success = false; - return ret; -} - -rcl_ret_t -wait_for_service_to_be_ready( - rcl_service_t * service, - rcl_context_t * context, - size_t max_tries, - int64_t period_ms, - bool & success) -{ - rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); - rcl_ret_t ret = - rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, 0, context, rcl_get_default_allocator()); - if (ret != RCL_RET_OK) { - success = false; - return ret; - } - size_t iteration = 0; - while (iteration < max_tries) { - ++iteration; - ret = rcl_wait_set_clear(&wait_set); - if (ret != RCL_RET_OK) { - break; - } - ret = rcl_wait_set_add_service(&wait_set, service, NULL); - if (ret != RCL_RET_OK) { - break; - } - ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); - if (ret == RCL_RET_TIMEOUT) { - continue; - } - if (ret != RCL_RET_OK) { - break; - } - for (size_t i = 0; i < wait_set.size_of_services; ++i) { - if (wait_set.services[i] && wait_set.services[i] == service) { - success = true; - ret = rcl_wait_set_fini(&wait_set); - return ret; - } - } - } - success = false; - ret = rcl_wait_set_fini(&wait_set); - return ret; -} - /* Basic nominal test of a service. */ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal) { @@ -200,10 +122,7 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal) EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; }); - bool success; - ret = wait_for_server_to_be_available(this->node_ptr, &client, 10, 1000, success); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_TRUE(success); + ASSERT_TRUE(wait_for_server_to_be_available(this->node_ptr, &client, 10, 1000)); // Initialize a request. test_msgs__srv__BasicTypes_Request client_request; @@ -222,9 +141,7 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal) test_msgs__srv__BasicTypes_Request__fini(&client_request); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = wait_for_service_to_be_ready(&service, context_ptr, 10, 100, success); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_TRUE(success); + ASSERT_TRUE(wait_for_service_to_be_ready(&service, context_ptr, 10, 100)); // This scope simulates the service responding in a different context so that we can // test take_request/send_response in a single-threaded, deterministic execution. @@ -252,9 +169,7 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal) ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; test_msgs__srv__BasicTypes_Request__fini(&service_request); } - ret = wait_for_service_to_be_ready(&service, context_ptr, 10, 100, success); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_FALSE(success); + ASSERT_FALSE(wait_for_service_to_be_ready(&service, context_ptr, 10, 100)); // Initialize the response owned by the client and take the response. test_msgs__srv__BasicTypes_Response client_response; diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 28978cfe8..d210637f0 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -19,14 +19,15 @@ #include #include "rcl/subscription.h" - #include "rcl/rcl.h" + #include "test_msgs/msg/basic_types.h" #include "test_msgs/msg/strings.h" #include "rosidl_runtime_c/string_functions.h" #include "osrf_testing_tools_cpp/scope_exit.hpp" #include "rcl/error_handling.h" +#include "common.hpp" #ifdef RMW_IMPLEMENTATION # define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX @@ -77,79 +78,6 @@ class CLASSNAME (TestSubscriptionFixture, RMW_IMPLEMENTATION) : public ::testing } }; -rcl_ret_t -wait_for_established_subscription( - const rcl_publisher_t * publisher, - size_t max_tries, - int64_t period_ms, - bool & success) -{ - size_t iteration = 0; - rcl_ret_t ret = RCL_RET_OK; - size_t subscription_count = 0; - while (iteration < max_tries) { - ++iteration; - ret = rcl_publisher_get_subscription_count(publisher, &subscription_count); - if (ret != RCL_RET_OK) { - success = false; - return ret; - } - if (subscription_count == 1) { - success = true; - return ret; - } - std::this_thread::sleep_for(std::chrono::milliseconds(period_ms)); - } - success = false; - return ret; -} - -rcl_ret_t -wait_for_subscription_to_be_ready( - rcl_subscription_t * subscription, - rcl_context_t * context, - size_t max_tries, - int64_t period_ms, - bool & success) -{ - rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); - rcl_ret_t ret = - rcl_wait_set_init(&wait_set, 1, 0, 0, 0, 0, 0, context, rcl_get_default_allocator()); - if (ret != RCL_RET_OK) { - success = false; - return ret; - } - size_t iteration = 0; - while (iteration < max_tries) { - ++iteration; - ret = rcl_wait_set_clear(&wait_set); - if (ret != RCL_RET_OK) { - break; - } - ret = rcl_wait_set_add_subscription(&wait_set, subscription, NULL); - if (ret != RCL_RET_OK) { - break; - } - ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms)); - if (ret == RCL_RET_TIMEOUT) { - continue; - } - if (ret != RCL_RET_OK) { - break; - } - for (size_t i = 0; i < wait_set.size_of_subscriptions; ++i) { - if (wait_set.subscriptions[i] && wait_set.subscriptions[i] == subscription) { - success = true; - ret = rcl_wait_set_fini(&wait_set); - return ret; - } - } - } - success = false; - ret = rcl_wait_set_fini(&wait_set); - return ret; -} - /* Test subscription init, fini and is_valid functions */ TEST_F( @@ -209,10 +137,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription }); rcl_reset_error(); - bool success; - ret = wait_for_established_subscription(&publisher, 10, 100, success); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_TRUE(success); + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); #ifdef RMW_TIMESTAMPS_SUPPORTED rcl_time_point_value_t pre_publish_time; EXPECT_EQ( @@ -227,9 +152,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription test_msgs__msg__BasicTypes__fini(&msg); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } - ret = wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_TRUE(success); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); { test_msgs__msg__BasicTypes msg; test_msgs__msg__BasicTypes__init(&msg); @@ -284,10 +207,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; }); - bool success; - ret = wait_for_established_subscription(&publisher, 10, 100, success); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_TRUE(success); + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); const char * test_string = "testing"; { test_msgs__msg__Strings msg; @@ -297,9 +217,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription test_msgs__msg__Strings__fini(&msg); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } - ret = wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_TRUE(success); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); { test_msgs__msg__Strings msg; test_msgs__msg__Strings__init(&msg); @@ -341,10 +259,7 @@ TEST_F( rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; }); - bool success; - ret = wait_for_established_subscription(&publisher, 10, 100, success); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_TRUE(success); + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); const char * test_string = "testing"; { test_msgs__msg__Strings msg; @@ -356,9 +271,7 @@ TEST_F( test_msgs__msg__Strings__fini(&msg); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } - ret = wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_TRUE(success); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); auto allocator = rcutils_get_default_allocator(); { size_t size = 1; @@ -507,17 +420,12 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription }); rcl_reset_error(); - bool success; - ret = wait_for_established_subscription(&publisher, 10, 100, success); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_TRUE(success); + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); { ret = rcl_publish_serialized_message(&publisher, &serialized_msg, nullptr); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } - ret = wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_TRUE(success); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); { rcl_serialized_message_t serialized_msg_rcv = rmw_get_zero_initialized_serialized_message(); initial_capacity_ser = 0u; @@ -561,10 +469,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription rcl_ret_t ret = rcl_subscription_fini(&subscription, this->node_ptr); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; }); - bool success; - ret = wait_for_established_subscription(&publisher, 10, 100, success); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_TRUE(success); + ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100)); const char * test_string = "testing"; { test_msgs__msg__Strings msg; @@ -574,9 +479,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription test_msgs__msg__Strings__fini(&msg); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; } - ret = wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success); - ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_TRUE(success); + ASSERT_TRUE(wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100)); { test_msgs__msg__Strings msg; test_msgs__msg__Strings * msg_loaned;