niklas/rcl
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Convert sleep_for into appropriate logic in tests(#631)

Browse source 
Signed-off-by: Barry Xu <Barry.Xu@sony.com>
This commit is contained in:
Barry Xu 2020-05-14 21:36:22 +08:00 • committed by GitHub
parent 6d16465318
commit 73093590e6
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 326 additions and 268 deletions
Download patch file Download diff file Expand all files Collapse all files

8
rcl/test/CMakeLists.txt
View file

@ -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/wait_for_entity_helpers.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/wait_for_entity_helpers.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/wait_for_entity_helpers.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/wait_for_entity_helpers.cpp
LIBRARIES ${PROJECT_NAME}
AMENT_DEPENDENCIES ${rmw_implementation} "osrf_testing_tools_cpp" "test_msgs"
)

87
rcl/test/rcl/client_fixture.cpp
View file

@ -12,11 +12,6 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#include <chrono>
#include <stdexcept>
#include <string>
#include <thread>
#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 "wait_for_entity_helpers.hpp"
int main(int argc, char ** argv)
{

55
rcl/test/rcl/service_fixture.cpp
View file

@ -13,8 +13,6 @@
// limitations under the License.
#include <chrono>
#include <stdexcept>
#include <string>
#include <thread>
#include "rcutils/logging_macros.h"
@ -27,59 +25,8 @@
#include "osrf_testing_tools_cpp/scope_exit.hpp"
#include "rcl/error_handling.h"
#include "wait_for_entity_helpers.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)
{

56
rcl/test/rcl/test_service.cpp
View file

@ -14,18 +14,14 @@
#include <gtest/gtest.h>
#include <chrono>
#include <string>
#include <thread>
#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 "wait_for_entity_helpers.hpp"
#ifdef RMW_IMPLEMENTATION
# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
@ -76,45 +72,6 @@ public:
}
};
void
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());
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
{
rcl_ret_t ret = rcl_wait_set_fini(&wait_set);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
});
size_t iteration = 0;
do {
++iteration;
ret = rcl_wait_set_clear(&wait_set);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
ret = rcl_wait_set_add_service(&wait_set, service, NULL);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
if (ret == RCL_RET_TIMEOUT) {
continue;
}
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
for (size_t i = 0; i < wait_set.size_of_services; ++i) {
if (wait_set.services[i] && wait_set.services[i] == service) {
success = true;
return;
}
}
} while (iteration < max_tries);
success = false;
}
/* Basic nominal test of a service.
*/
TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal) {
@ -165,10 +122,7 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal)
EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
});
// TODO(wjwwood): add logic to wait for the connection to be established
// use count_services busy wait mechanism
// until then we will sleep for a short period of time
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
ASSERT_TRUE(wait_for_server_to_be_available(this->node_ptr, &client, 10, 1000));
// Initialize a request.
test_msgs__srv__BasicTypes_Request client_request;
@ -190,9 +144,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;
bool success;
wait_for_service_to_be_ready(&service, context_ptr, 10, 100, success);
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.
@ -234,7 +186,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);
}
wait_for_service_to_be_ready(&service, context_ptr, 10, 100, 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;

84
rcl/test/rcl/test_subscription.cpp
View file

@ -19,14 +19,15 @@
#include <thread>
#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 "wait_for_entity_helpers.hpp"
#ifdef RMW_IMPLEMENTATION
# define CLASSNAME_(NAME, SUFFIX) NAME ## __ ## SUFFIX
@ -77,45 +78,6 @@ public:
}
};
void
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());
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT(
{
rcl_ret_t ret = rcl_wait_set_fini(&wait_set);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
});
size_t iteration = 0;
do {
++iteration;
ret = rcl_wait_set_clear(&wait_set);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
ret = rcl_wait_set_add_subscription(&wait_set, subscription, NULL);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
ret = rcl_wait(&wait_set, RCL_MS_TO_NS(period_ms));
if (ret == RCL_RET_TIMEOUT) {
continue;
}
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
for (size_t i = 0; i < wait_set.size_of_subscriptions; ++i) {
if (wait_set.subscriptions[i] && wait_set.subscriptions[i] == subscription) {
success = true;
return;
}
}
} while (iteration < max_tries);
success = false;
}
/* Test subscription init, fini and is_valid functions
*/
TEST_F(
@ -175,10 +137,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
});
rcl_reset_error();
// TODO(wjwwood): add logic to wait for the connection to be established
// probably using the count_subscriptions busy wait mechanism
// until then we will sleep for a short period of time
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100));
#ifdef RMW_TIMESTAMPS_SUPPORTED
rcl_time_point_value_t pre_publish_time;
EXPECT_EQ(
@ -193,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;
}
bool success;
wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success);
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);
@ -250,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;
});
// TODO(wjwwood): add logic to wait for the connection to be established
// probably using the count_subscriptions busy wait mechanism
// until then we will sleep for a short period of time
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100));
const char * test_string = "testing";
{
test_msgs__msg__Strings msg;
@ -263,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;
}
bool success;
wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success);
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);
@ -307,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;
});
// TODO(wjwwood): add logic to wait for the connection to be established
// probably using the count_subscriptions busy wait mechanism
// until then we will sleep for a short period of time
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100));
const char * test_string = "testing";
{
test_msgs__msg__Strings msg;
@ -322,9 +271,7 @@ TEST_F(
test_msgs__msg__Strings__fini(&msg);
ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
}
bool success;
wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success);
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;
@ -473,14 +420,12 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
});
rcl_reset_error();
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
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;
}
bool success;
wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success);
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;
@ -524,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;
});
// TODO(wjwwood): add logic to wait for the connection to be established
// probably using the count_subscriptions busy wait mechanism
// until then we will sleep for a short period of time
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
ASSERT_TRUE(wait_for_established_subscription(&publisher, 10, 100));
const char * test_string = "testing";
{
test_msgs__msg__Strings msg;
@ -537,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;
}
bool success;
wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success);
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;

238
rcl/test/rcl/wait_for_entity_helpers.cpp Normal file
View file

@ -0,0 +1,238 @@
// 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 "wait_for_entity_helpers.hpp"
#include <chrono>
#include <stdexcept>
#include <thread>
#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 > 0) {
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;
}

66
rcl/test/rcl/wait_for_entity_helpers.hpp Normal file
View file

@ -0,0 +1,66 @@
// 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__WAIT_FOR_ENTITY_HELPERS_HPP_
#define RCL__WAIT_FOR_ENTITY_HELPERS_HPP_
#include "rcl/client.h"
#include "rcl/service.h"
#include "rcl/rcl.h"
/// Wait for a server to be available for `client`, by trying at most `max_tries` times
/// with a `period_ms` period.
bool
wait_for_server_to_be_available(
rcl_node_t * node,
rcl_client_t * client,
size_t max_tries,
int64_t period_ms);
/// Wait for `client` to be ready, i.e. a response is available to be handled.
/// It's tried at most `max_tries` times with a period of `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);
/// Wait for service to be ready, i.e. a request is available to be handled.
/// It's tried at most `max_tries` times with a period of `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);
/// Wait for a publisher to get one or more established subscriptions
/// by trying at most `max_tries` times with a `period_ms` period.
bool
wait_for_established_subscription(
const rcl_publisher_t * publisher,
size_t max_tries,
int64_t period_ms);
/// Wait a subscription to be ready, i.e. a message is ready to be handled,
/// by trying at least `max_tries` times with a `period_ms` period.
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__WAIT_FOR_ENTITY_HELPERS_HPP_