From f2c50e418fcf8a622a9ca974bf9347ad5bec3ac1 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 16 Aug 2018 16:09:40 -0700 Subject: [PATCH 1/5] Consolidate functions to clear wait set Add rcl_wait_set_clear() Remove rcl_wait_set_clear_subscriptions() rcl_wait_set_clear_guard_conditions() rcl_wait_set_clear_clients() rcl_wait_set_clear_services() rcl_wait_set_clear_timers() --- rcl/include/rcl/wait.h | 54 ++--------- rcl/src/rcl/wait.c | 146 ++++++++++------------------- rcl/test/rcl/client_fixture.cpp | 4 +- rcl/test/rcl/service_fixture.cpp | 4 +- rcl/test/rcl/test_graph.cpp | 6 +- rcl/test/rcl/test_service.cpp | 2 +- rcl/test/rcl/test_subscription.cpp | 2 +- rcl/test/rcl/test_wait.cpp | 2 +- 8 files changed, 66 insertions(+), 154 deletions(-) diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index 62900a8f1..bd9e21373 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -209,10 +209,10 @@ rcl_wait_set_add_subscription( rcl_wait_set_t * wait_set, const rcl_subscription_t * subscription); -/// Remove (sets to `NULL`) the subscriptions in the wait set. +/// Remove (sets to `NULL`) all entities in the wait set. /** * This function should be used after passing using rcl_wait, but before - * adding new subscriptions to the set. + * adding new entities to the set. * Sets all of the entries in the underlying rmw array to `NULL`, and sets the * count in the rmw array to `0`. * @@ -226,7 +226,7 @@ rcl_wait_set_add_subscription( * Uses Atomics | No * Lock-Free | Yes * - * \param[inout] wait_set struct to have its subscriptions cleared + * \param[inout] wait_set struct to have its entities cleared * \return `RCL_RET_OK` if cleared successfully, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_WAIT_SET_INVALID` if the wait set is zero initialized, or @@ -235,7 +235,7 @@ rcl_wait_set_add_subscription( RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t -rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set); +rcl_wait_set_clear(rcl_wait_set_t * wait_set); /// Reallocate space for the subscriptions in the wait set. /** @@ -248,7 +248,7 @@ rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set); * wait set's initialization. * * After calling this function all values in the set will be set to `NULL`, - * effectively the same as calling rcl_wait_set_clear_subscriptions(). + * effectively the same as calling rcl_wait_set_clear(). * Similarly, the underlying rmw representation is reallocated and reset: * all entries are set to `NULL` and the count is set to zero. * @@ -288,16 +288,6 @@ rcl_wait_set_add_guard_condition( rcl_wait_set_t * wait_set, const rcl_guard_condition_t * guard_condition); -/// Remove (sets to `NULL`) the guard conditions in the wait set. -/** - * This function behaves exactly the same as for subscriptions. - * \see rcl_wait_set_clear_subscriptions - */ -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set); - /// Reallocate space for the guard conditions in the wait set. /** * This function behaves exactly the same as for subscriptions. @@ -320,16 +310,6 @@ rcl_wait_set_add_timer( rcl_wait_set_t * wait_set, const rcl_timer_t * timer); -/// Remove (sets to `NULL`) the timers in the wait set. -/** - * This function behaves exactly the same as for subscriptions. - * \see rcl_wait_set_clear_subscriptions - */ -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_wait_set_clear_timers(rcl_wait_set_t * wait_set); - /// Reallocate space for the timers in the wait set. /** * This function behaves exactly the same as for subscriptions. @@ -352,16 +332,6 @@ rcl_wait_set_add_client( rcl_wait_set_t * wait_set, const rcl_client_t * client); -/// Remove (sets to `NULL`) the clients in the wait set. -/** - * This function behaves exactly the same as for subscriptions. - * \see rcl_wait_set_clear_subscriptions - */ -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_wait_set_clear_clients(rcl_wait_set_t * wait_set); - /// Reallocate space for the clients in the wait set. /** * This function behaves exactly the same as for subscriptions. @@ -384,16 +354,6 @@ rcl_wait_set_add_service( rcl_wait_set_t * wait_set, const rcl_service_t * service); -/// Remove (sets to `NULL`) the services in the wait set. -/** - * This function behaves exactly the same as for subscriptions. - * \see rcl_wait_set_clear_subscriptions - */ -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_wait_set_clear_services(rcl_wait_set_t * wait_set); - /// Reallocate space for the services in the wait set. /** * This function behaves exactly the same as for subscriptions. @@ -430,9 +390,7 @@ rcl_wait_set_resize_services(rcl_wait_set_t * wait_set, size_t size); * rcl_ret_t ret = rcl_wait_set_init(&wait_set, 2, 1, 0, 0, 0, rcl_get_default_allocator()); * // ... error handling * do { - * ret = rcl_wait_set_clear_subscriptions(&wait_set); - * // ... error handling - * ret = rcl_wait_set_clear_guard_conditions(&wait_set); + * ret = rcl_wait_set_clear(&wait_set); * // ... error handling * ret = rcl_wait_set_add_subscription(&wait_set, &sub1); * // ... error handling diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index e3a0e3454..e7e651ff2 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -33,15 +33,20 @@ extern "C" typedef struct rcl_wait_set_impl_t { + // number of subscriptions that have been added to the wait set size_t subscription_index; rmw_subscriptions_t rmw_subscriptions; + // number of guard_conditions that have been added to the wait set size_t guard_condition_index; rmw_guard_conditions_t rmw_guard_conditions; + // number of clients that have been added to the wait set size_t client_index; rmw_clients_t rmw_clients; + // number of services that have been added to the wait set size_t service_index; rmw_services_t rmw_services; rmw_wait_set_t * rmw_wait_set; + // number of timers that have been added to the wait set size_t timer_index; rcl_allocator_t allocator; } rcl_wait_set_impl_t; @@ -158,50 +163,30 @@ rcl_wait_set_init( fail_ret = ret; goto fail; } - if ((ret = rcl_wait_set_clear_subscriptions(wait_set)) != RCL_RET_OK) { - fail_ret = ret; - goto fail; - } // Initialize guard condition space. ret = rcl_wait_set_resize_guard_conditions(wait_set, number_of_guard_conditions); if (ret != RCL_RET_OK) { fail_ret = ret; goto fail; } - if ((ret = rcl_wait_set_clear_guard_conditions(wait_set)) != RCL_RET_OK) { - fail_ret = ret; - goto fail; - } // Initialize timer space. ret = rcl_wait_set_resize_timers(wait_set, number_of_timers); if (ret != RCL_RET_OK) { fail_ret = ret; goto fail; } - if ((ret = rcl_wait_set_clear_timers(wait_set)) != RCL_RET_OK) { - fail_ret = ret; - goto fail; - } // Initialize client space. ret = rcl_wait_set_resize_clients(wait_set, number_of_clients); if (ret != RCL_RET_OK) { fail_ret = ret; goto fail; } - if ((ret = rcl_wait_set_clear_clients(wait_set)) != RCL_RET_OK) { - fail_ret = ret; - goto fail; - } // Initialize service space. ret = rcl_wait_set_resize_services(wait_set, number_of_services); if (ret != RCL_RET_OK) { fail_ret = ret; goto fail; } - if ((ret = rcl_wait_set_clear_services(wait_set)) != RCL_RET_OK) { - fail_ret = ret; - goto fail; - } return RCL_RET_OK; fail: if (__wait_set_is_valid(wait_set)) { @@ -267,24 +252,23 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al wait_set->impl->RMWCount++; #define SET_CLEAR(Type) \ - RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); \ - if (!__wait_set_is_valid(wait_set)) { \ - RCL_SET_ERROR_MSG("wait set is invalid", rcl_get_default_allocator()); \ - return RCL_RET_WAIT_SET_INVALID; \ - } \ - memset( \ - (void *)wait_set->Type ## s, \ - 0, \ - sizeof(rcl_ ## Type ## _t *) * wait_set->size_of_ ## Type ## s); \ - wait_set->impl->Type ## _index = 0; + do { \ + memset( \ + (void *)wait_set->Type ## s, \ + 0, \ + sizeof(rcl_ ## Type ## _t *) * wait_set->size_of_ ## Type ## s); \ + wait_set->impl->Type ## _index = 0; \ + } while (false) #define SET_CLEAR_RMW(Type, RMWStorage, RMWCount) \ - /* Also clear the rmw storage. */ \ - memset( \ - wait_set->impl->RMWStorage, \ - 0, \ - sizeof(void *) * wait_set->impl->RMWCount); \ - wait_set->impl->RMWCount = 0; + do { \ + /* Also clear the rmw storage. */ \ + memset( \ + wait_set->impl->RMWStorage, \ + 0, \ + sizeof(void *) * wait_set->impl->RMWCount); \ + wait_set->impl->RMWCount = 0; \ + } while (false) #define SET_RESIZE(Type, ExtraDealloc, ExtraRealloc) \ RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); \ @@ -357,13 +341,35 @@ rcl_wait_set_add_subscription( * count in the rmw array to 0. */ rcl_ret_t -rcl_wait_set_clear_subscriptions(rcl_wait_set_t * wait_set) +rcl_wait_set_clear(rcl_wait_set_t * wait_set) { - SET_CLEAR(subscription) + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); + RCL_CHECK_ARGUMENT_FOR_NULL( + wait_set->impl, RCL_RET_WAIT_SET_INVALID, rcl_get_default_allocator()); + + SET_CLEAR(subscription); + SET_CLEAR(guard_condition); + SET_CLEAR(client); + SET_CLEAR(service); + SET_CLEAR(timer); + SET_CLEAR_RMW( subscription, rmw_subscriptions.subscribers, - rmw_subscriptions.subscriber_count) + rmw_subscriptions.subscriber_count); + SET_CLEAR_RMW( + guard_condition, + rmw_guard_conditions.guard_conditions, + rmw_guard_conditions.guard_condition_count); + SET_CLEAR_RMW( + clients, + rmw_clients.clients, + rmw_clients.client_count); + SET_CLEAR_RMW( + services, + rmw_services.services, + rmw_services.service_count); + return RCL_RET_OK; } @@ -395,17 +401,6 @@ rcl_wait_set_add_guard_condition( return RCL_RET_OK; } -rcl_ret_t -rcl_wait_set_clear_guard_conditions(rcl_wait_set_t * wait_set) -{ - SET_CLEAR(guard_condition) - SET_CLEAR_RMW( - guard_condition, - rmw_guard_conditions.guard_conditions, - rmw_guard_conditions.guard_condition_count) - return RCL_RET_OK; -} - rcl_ret_t rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size) { @@ -430,13 +425,6 @@ rcl_wait_set_add_timer( return RCL_RET_OK; } -rcl_ret_t -rcl_wait_set_clear_timers(rcl_wait_set_t * wait_set) -{ - SET_CLEAR(timer) - return RCL_RET_OK; -} - rcl_ret_t rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size) { @@ -453,17 +441,6 @@ rcl_wait_set_add_client( return RCL_RET_OK; } -rcl_ret_t -rcl_wait_set_clear_clients(rcl_wait_set_t * wait_set) -{ - SET_CLEAR(client) - SET_CLEAR_RMW( - clients, - rmw_clients.clients, - rmw_clients.client_count) - return RCL_RET_OK; -} - rcl_ret_t rcl_wait_set_resize_clients(rcl_wait_set_t * wait_set, size_t size) { @@ -485,17 +462,6 @@ rcl_wait_set_add_service( return RCL_RET_OK; } -rcl_ret_t -rcl_wait_set_clear_services(rcl_wait_set_t * wait_set) -{ - SET_CLEAR(service) - SET_CLEAR_RMW( - services, - rmw_services.services, - rmw_services.service_count) - return RCL_RET_OK; -} - rcl_ret_t rcl_wait_set_resize_services(rcl_wait_set_t * wait_set, size_t size) { @@ -627,23 +593,7 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) } } // Check for timeout, return RCL_RET_TIMEOUT only if it wasn't a timer. - if (ret == RMW_RET_TIMEOUT) { - // Assume none were set (because timeout was reached first), and clear all. - rcl_ret_t rcl_ret; - // This next line prevents "assigned but never used" warnings in Release mode. - (void)rcl_ret; // NO LINT - rcl_ret = rcl_wait_set_clear_subscriptions(wait_set); - assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set. - rcl_ret = rcl_wait_set_clear_guard_conditions(wait_set); - assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set. - rcl_ret = rcl_wait_set_clear_services(wait_set); - assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set. - rcl_ret = rcl_wait_set_clear_clients(wait_set); - assert(rcl_ret == RCL_RET_OK); // Defensive, shouldn't fail with valid wait_set. - if (!is_timer_timeout) { - return RCL_RET_TIMEOUT; - } - } else if (ret != RMW_RET_OK) { + if (ret != RMW_RET_OK && ret != RMW_RET_TIMEOUT) { RCL_SET_ERROR_MSG(rmw_get_error_string_safe(), wait_set->impl->allocator); return RCL_RET_ERROR; } @@ -681,6 +631,10 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) wait_set->services[i] = NULL; } } + + if (ret == RMW_RET_TIMEOUT && !is_timer_timeout) { + return RCL_RET_TIMEOUT; + } return RCL_RET_OK; } diff --git a/rcl/test/rcl/client_fixture.cpp b/rcl/test/rcl/client_fixture.cpp index d5249cda3..3bbe90e9e 100644 --- a/rcl/test/rcl/client_fixture.cpp +++ b/rcl/test/rcl/client_fixture.cpp @@ -77,9 +77,9 @@ wait_for_client_to_be_ready( size_t iteration = 0; do { ++iteration; - if (rcl_wait_set_clear_clients(&wait_set) != RCL_RET_OK) { + if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in wait_set_clear_clients: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string_safe()) return false; } if (rcl_wait_set_add_client(&wait_set, client) != RCL_RET_OK) { diff --git a/rcl/test/rcl/service_fixture.cpp b/rcl/test/rcl/service_fixture.cpp index d3d2e0962..61b129a03 100644 --- a/rcl/test/rcl/service_fixture.cpp +++ b/rcl/test/rcl/service_fixture.cpp @@ -50,9 +50,9 @@ wait_for_service_to_be_ready( size_t iteration = 0; do { ++iteration; - if (rcl_wait_set_clear_services(&wait_set) != RCL_RET_OK) { + if (rcl_wait_set_clear(&wait_set) != RCL_RET_OK) { RCUTILS_LOG_ERROR_NAMED( - ROS_PACKAGE_NAME, "Error in wait_set_clear_services: %s", rcl_get_error_string_safe()) + ROS_PACKAGE_NAME, "Error in wait_set_clear: %s", rcl_get_error_string_safe()) return false; } if (rcl_wait_set_add_service(&wait_set, service) != RCL_RET_OK) { diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp index 3d606fc32..41e2fa7b9 100644 --- a/rcl/test/rcl/test_graph.cpp +++ b/rcl/test/rcl/test_graph.cpp @@ -285,7 +285,7 @@ check_graph_state( // Don't wait for the graph to change on the last loop because we won't check again. continue; } - ret = rcl_wait_set_clear_guard_conditions(wait_set_ptr); + ret = rcl_wait_set_clear(wait_set_ptr); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); ret = rcl_wait_set_add_guard_condition(wait_set_ptr, graph_guard_condition); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); @@ -444,7 +444,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_conditi 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_guard_conditions(this->wait_set_ptr); + ret = rcl_wait_set_clear(this->wait_set_ptr); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); ret = rcl_wait_set_add_guard_condition(this->wait_set_ptr, graph_guard_condition); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); @@ -502,7 +502,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_rcl_service_server_ if (time_to_sleep > min_sleep) { time_to_sleep = min_sleep; } - rcl_ret_t ret = rcl_wait_set_clear_guard_conditions(this->wait_set_ptr); + rcl_ret_t ret = rcl_wait_set_clear(this->wait_set_ptr); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); ret = rcl_wait_set_add_guard_condition(this->wait_set_ptr, graph_guard_condition); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); diff --git a/rcl/test/rcl/test_service.cpp b/rcl/test/rcl/test_service.cpp index 55b47398c..238c25dff 100644 --- a/rcl/test/rcl/test_service.cpp +++ b/rcl/test/rcl/test_service.cpp @@ -78,7 +78,7 @@ wait_for_service_to_be_ready( size_t iteration = 0; do { ++iteration; - ret = rcl_wait_set_clear_services(&wait_set); + ret = rcl_wait_set_clear(&wait_set); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); ret = rcl_wait_set_add_service(&wait_set, service); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index 775beae0d..6925f7e46 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -78,7 +78,7 @@ wait_for_subscription_to_be_ready( size_t iteration = 0; do { ++iteration; - ret = rcl_wait_set_clear_subscriptions(&wait_set); + ret = rcl_wait_set_clear(&wait_set); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); ret = rcl_wait_set_add_subscription(&wait_set, subscription); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index 081277b73..ad5a7abe5 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -287,7 +287,7 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), multi_wait_set_threade while (wake_try_count < retry_limit) { wake_try_count++; rcl_ret_t ret; - ret = rcl_wait_set_clear_guard_conditions(&test_set.wait_set); + ret = rcl_wait_set_clear(&test_set.wait_set); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); ret = rcl_wait_set_add_guard_condition(&test_set.wait_set, &test_set.guard_condition); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); From 66f0630679e4a2865d48d2deeeab014ddff7657c Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 16 Aug 2018 17:52:50 -0700 Subject: [PATCH 2/5] Consolidate functions to resize wait set Add rcl_wait_set_resize() Removed rcl_wait_set_resize_subscriptions() rcl_wait_set_resize_guard_conditions() rcl_wait_set_resize_timers() rcl_wait_set_resize_clients() rcl_wait_set_resize_services() --- rcl/include/rcl/wait.h | 61 +++---------- rcl/src/rcl/wait.c | 183 +++++++++++++------------------------ rcl/test/rcl/test_wait.cpp | 8 +- 3 files changed, 84 insertions(+), 168 deletions(-) diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index bd9e21373..890b4b275 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -237,10 +237,9 @@ RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_clear(rcl_wait_set_t * wait_set); -/// Reallocate space for the subscriptions in the wait set. +/// Reallocate space for entities in the wait set. /** - * This function will deallocate and reallocate the memory for the - * subscriptions set. + * This function will deallocate and reallocate the memory for all entity sets. * * A size of 0 will just deallocate the memory and assign `NULL` to the array. * @@ -264,8 +263,12 @@ rcl_wait_set_clear(rcl_wait_set_t * wait_set); * Uses Atomics | No * Lock-Free | Yes * - * \param[inout] wait_set struct to have its subscriptions cleared - * \param[in] size a size for the new set + * \param[inout] wait_set struct to be resized + * \param[in] subscriptions_size a size for the new subscriptions set + * \param[in] guard_conditions_size a size for the new subscriptions set + * \param[in] clients_size a size for the new subscriptions set + * \param[in] services_size a size for the new subscriptions set + * \param[in] timers_size a size for the new subscriptions set * \return `RCL_RET_OK` if resized successfully, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or @@ -274,7 +277,13 @@ rcl_wait_set_clear(rcl_wait_set_t * wait_set); RCL_PUBLIC RCL_WARN_UNUSED rcl_ret_t -rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size); +rcl_wait_set_resize( + rcl_wait_set_t * wait_set, + size_t subscriptions_size, + size_t guard_conditions_size, + size_t timers_size, + size_t clients_size, + size_t services_size); /// Store a pointer to the guard condition in the next empty spot in the set. /** @@ -288,16 +297,6 @@ rcl_wait_set_add_guard_condition( rcl_wait_set_t * wait_set, const rcl_guard_condition_t * guard_condition); -/// Reallocate space for the guard conditions in the wait set. -/** - * This function behaves exactly the same as for subscriptions. - * \see rcl_wait_set_resize_subscriptions - */ -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size); - /// Store a pointer to the timer in the next empty spot in the set. /** * This function behaves exactly the same as for subscriptions. @@ -310,16 +309,6 @@ rcl_wait_set_add_timer( rcl_wait_set_t * wait_set, const rcl_timer_t * timer); -/// Reallocate space for the timers in the wait set. -/** - * This function behaves exactly the same as for subscriptions. - * \see rcl_wait_set_resize_subscriptions - */ -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size); - /// Store a pointer to the client in the next empty spot in the set. /** * This function behaves exactly the same as for subscriptions. @@ -332,16 +321,6 @@ rcl_wait_set_add_client( rcl_wait_set_t * wait_set, const rcl_client_t * client); -/// Reallocate space for the clients in the wait set. -/** - * This function behaves exactly the same as for subscriptions. - * \see rcl_wait_set_resize_subscriptions - */ -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_wait_set_resize_clients(rcl_wait_set_t * wait_set, size_t size); - /// Store a pointer to the client in the next empty spot in the set. /** * This function behaves exactly the same as for subscriptions. @@ -354,16 +333,6 @@ rcl_wait_set_add_service( rcl_wait_set_t * wait_set, const rcl_service_t * service); -/// Reallocate space for the services in the wait set. -/** - * This function behaves exactly the same as for subscriptions. - * \see rcl_wait_set_resize_subscriptions - */ -RCL_PUBLIC -RCL_WARN_UNUSED -rcl_ret_t -rcl_wait_set_resize_services(rcl_wait_set_t * wait_set, size_t size); - /// Block until the wait set is ready or until the timeout has been exceeded. /** * This function will collect the items in the rcl_wait_set_t and pass them diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index e7e651ff2..c86bdfdc5 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -80,27 +80,7 @@ static void __wait_set_clean_up(rcl_wait_set_t * wait_set, rcl_allocator_t allocator) { if (wait_set->subscriptions) { - rcl_ret_t ret = rcl_wait_set_resize_subscriptions(wait_set, 0); - (void)ret; // NO LINT - assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0. - } - if (wait_set->guard_conditions) { - rcl_ret_t ret = rcl_wait_set_resize_guard_conditions(wait_set, 0); - (void)ret; // NO LINT - assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0. - } - if (wait_set->timers) { - rcl_ret_t ret = rcl_wait_set_resize_timers(wait_set, 0); - (void)ret; // NO LINT - assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0. - } - if (wait_set->clients) { - rcl_ret_t ret = rcl_wait_set_resize_clients(wait_set, 0); - (void)ret; // NO LINT - assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0. - } - if (wait_set->services) { - rcl_ret_t ret = rcl_wait_set_resize_services(wait_set, 0); + rcl_ret_t ret = rcl_wait_set_resize(wait_set, 0u, 0u, 0u, 0u, 0u); (void)ret; // NO LINT assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0. } @@ -158,32 +138,10 @@ rcl_wait_set_init( // Set allocator. wait_set->impl->allocator = allocator; // Initialize subscription space. - rcl_ret_t ret; - if ((ret = rcl_wait_set_resize_subscriptions(wait_set, number_of_subscriptions)) != RCL_RET_OK) { - fail_ret = ret; - goto fail; - } - // Initialize guard condition space. - ret = rcl_wait_set_resize_guard_conditions(wait_set, number_of_guard_conditions); - if (ret != RCL_RET_OK) { - fail_ret = ret; - goto fail; - } - // Initialize timer space. - ret = rcl_wait_set_resize_timers(wait_set, number_of_timers); - if (ret != RCL_RET_OK) { - fail_ret = ret; - goto fail; - } - // Initialize client space. - ret = rcl_wait_set_resize_clients(wait_set, number_of_clients); - if (ret != RCL_RET_OK) { - fail_ret = ret; - goto fail; - } - // Initialize service space. - ret = rcl_wait_set_resize_services(wait_set, number_of_services); - if (ret != RCL_RET_OK) { + rcl_ret_t ret = rcl_wait_set_resize( + wait_set, number_of_subscriptions, number_of_guard_conditions, number_of_timers, + number_of_clients, number_of_services); + if (RCL_RET_OK != ret) { fail_ret = ret; goto fail; } @@ -271,33 +229,28 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al } while (false) #define SET_RESIZE(Type, ExtraDealloc, ExtraRealloc) \ - RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); \ - RCL_CHECK_FOR_NULL_WITH_MSG( \ - wait_set->impl, "wait set is invalid", \ - return RCL_RET_WAIT_SET_INVALID, rcl_get_default_allocator()); \ - if (size == wait_set->size_of_ ## Type ## s) { \ - return RCL_RET_OK; \ - } \ - rcl_allocator_t allocator = wait_set->impl->allocator; \ - wait_set->size_of_ ## Type ## s = 0; \ - wait_set->impl->Type ## _index = 0; \ - if (size == 0) { \ - if (wait_set->Type ## s) { \ - allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \ - wait_set->Type ## s = NULL; \ + do { \ + rcl_allocator_t allocator = wait_set->impl->allocator; \ + wait_set->size_of_ ## Type ## s = 0; \ + wait_set->impl->Type ## _index = 0; \ + if (0u == Type ## s_size) { \ + if (wait_set->Type ## s) { \ + allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \ + wait_set->Type ## s = NULL; \ + } \ + ExtraDealloc \ + } else { \ + wait_set->Type ## s = (const rcl_ ## Type ## _t **)allocator.reallocate( \ + (void *)wait_set->Type ## s, sizeof(rcl_ ## Type ## _t *) * Type ## s_size, \ + allocator.state); \ + RCL_CHECK_FOR_NULL_WITH_MSG( \ + wait_set->Type ## s, "allocating memory failed", \ + return RCL_RET_BAD_ALLOC, wait_set->impl->allocator); \ + memset((void *)wait_set->Type ## s, 0, sizeof(rcl_ ## Type ## _t *) * Type ## s_size); \ + wait_set->size_of_ ## Type ## s = Type ## s_size; \ + ExtraRealloc \ } \ - ExtraDealloc \ - } else { \ - wait_set->Type ## s = (const rcl_ ## Type ## _t **)allocator.reallocate( \ - (void *)wait_set->Type ## s, sizeof(rcl_ ## Type ## _t *) * size, allocator.state); \ - RCL_CHECK_FOR_NULL_WITH_MSG( \ - wait_set->Type ## s, "allocating memory failed", \ - return RCL_RET_BAD_ALLOC, wait_set->impl->allocator); \ - memset((void *)wait_set->Type ## s, 0, sizeof(rcl_ ## Type ## _t *) * size); \ - wait_set->size_of_ ## Type ## s = size; \ - ExtraRealloc \ - } \ - return RCL_RET_OK; + } while (false) #define SET_RESIZE_RMW_DEALLOC(RMWStorage, RMWCount) \ /* Also deallocate the rmw storage. */ \ @@ -311,14 +264,14 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al /* Also resize the rmw storage. */ \ wait_set->impl->RMWCount = 0; \ wait_set->impl->RMWStorage = (void **)allocator.reallocate( \ - wait_set->impl->RMWStorage, sizeof(void *) * size, allocator.state); \ + wait_set->impl->RMWStorage, sizeof(void *) * Type ## s_size, allocator.state); \ if (!wait_set->impl->RMWStorage) { \ allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \ wait_set->size_of_ ## Type ## s = 0; \ RCL_SET_ERROR_MSG("allocating memory failed", wait_set->impl->allocator); \ return RCL_RET_BAD_ALLOC; \ } \ - memset(wait_set->impl->RMWStorage, 0, sizeof(void *) * size); + memset(wait_set->impl->RMWStorage, 0, sizeof(void *) * Type ## s_size); /* Implementation-specific notes: * @@ -379,15 +332,48 @@ rcl_wait_set_clear(rcl_wait_set_t * wait_set) * all entries are set to null and the count is set to zero. */ rcl_ret_t -rcl_wait_set_resize_subscriptions(rcl_wait_set_t * wait_set, size_t size) +rcl_wait_set_resize( + rcl_wait_set_t * wait_set, + size_t subscriptions_size, + size_t guard_conditions_size, + size_t timers_size, + size_t clients_size, + size_t services_size) { + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_INVALID_ARGUMENT, rcl_get_default_allocator()); + RCL_CHECK_ARGUMENT_FOR_NULL( + wait_set->impl, RCL_RET_WAIT_SET_INVALID, rcl_get_default_allocator()); SET_RESIZE( subscription, SET_RESIZE_RMW_DEALLOC( rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count), SET_RESIZE_RMW_REALLOC( subscription, rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count) - ) + ); + SET_RESIZE( + guard_condition, + SET_RESIZE_RMW_DEALLOC( + rmw_guard_conditions.guard_conditions, + rmw_guard_conditions.guard_condition_count), + SET_RESIZE_RMW_REALLOC( + guard_condition, + rmw_guard_conditions.guard_conditions, + rmw_guard_conditions.guard_condition_count) + ); + SET_RESIZE(timer,;,;); // NOLINT + SET_RESIZE(client, + SET_RESIZE_RMW_DEALLOC( + rmw_clients.clients, rmw_clients.client_count), + SET_RESIZE_RMW_REALLOC( + client, rmw_clients.clients, rmw_clients.client_count) + ); + SET_RESIZE(service, + SET_RESIZE_RMW_DEALLOC( + rmw_services.services, rmw_services.service_count), + SET_RESIZE_RMW_REALLOC( + service, rmw_services.services, rmw_services.service_count) + ); + return RCL_RET_OK; } rcl_ret_t @@ -401,21 +387,6 @@ rcl_wait_set_add_guard_condition( return RCL_RET_OK; } -rcl_ret_t -rcl_wait_set_resize_guard_conditions(rcl_wait_set_t * wait_set, size_t size) -{ - SET_RESIZE( - guard_condition, - SET_RESIZE_RMW_DEALLOC( - rmw_guard_conditions.guard_conditions, - rmw_guard_conditions.guard_condition_count), - SET_RESIZE_RMW_REALLOC( - guard_condition, - rmw_guard_conditions.guard_conditions, - rmw_guard_conditions.guard_condition_count) - ) -} - rcl_ret_t rcl_wait_set_add_timer( rcl_wait_set_t * wait_set, @@ -425,12 +396,6 @@ rcl_wait_set_add_timer( return RCL_RET_OK; } -rcl_ret_t -rcl_wait_set_resize_timers(rcl_wait_set_t * wait_set, size_t size) -{ - SET_RESIZE(timer,;,;) // NOLINT -} - rcl_ret_t rcl_wait_set_add_client( rcl_wait_set_t * wait_set, @@ -441,17 +406,6 @@ rcl_wait_set_add_client( return RCL_RET_OK; } -rcl_ret_t -rcl_wait_set_resize_clients(rcl_wait_set_t * wait_set, size_t size) -{ - SET_RESIZE(client, - SET_RESIZE_RMW_DEALLOC( - rmw_clients.clients, rmw_clients.client_count), - SET_RESIZE_RMW_REALLOC( - client, rmw_clients.clients, rmw_clients.client_count) - ) -} - rcl_ret_t rcl_wait_set_add_service( rcl_wait_set_t * wait_set, @@ -462,17 +416,6 @@ rcl_wait_set_add_service( return RCL_RET_OK; } -rcl_ret_t -rcl_wait_set_resize_services(rcl_wait_set_t * wait_set, size_t size) -{ - SET_RESIZE(service, - SET_RESIZE_RMW_DEALLOC( - rmw_services.services, rmw_services.service_count), - SET_RESIZE_RMW_REALLOC( - service, rmw_services.services, rmw_services.service_count) - ) -} - rcl_ret_t rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) { diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index ad5a7abe5..d70dda2f0 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -59,13 +59,17 @@ class CLASSNAME (WaitSetTestFixture, RMW_IMPLEMENTATION) : public ::testing::Tes TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), test_resize_to_zero) { // Initialize a wait set with a subscription and then resize it to zero. 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, rcl_get_default_allocator()); + rcl_ret_t ret = rcl_wait_set_init(&wait_set, 1, 1, 1, 1, 1, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); - ret = rcl_wait_set_resize_subscriptions(&wait_set, 0); + ret = rcl_wait_set_resize(&wait_set, 0u, 0u, 0u, 0u, 0u); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); EXPECT_EQ(wait_set.size_of_subscriptions, 0ull); + EXPECT_EQ(wait_set.size_of_guard_conditions, 0ull); + EXPECT_EQ(wait_set.size_of_clients, 0ull); + EXPECT_EQ(wait_set.size_of_services, 0ull); + EXPECT_EQ(wait_set.size_of_timers, 0ull); ret = rcl_wait_set_fini(&wait_set); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string_safe(); From 5d578e6e3e85320a2ebd12f43cd0447199746091 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 23 Aug 2018 13:53:34 -0700 Subject: [PATCH 3/5] 0u -> 0 --- rcl/src/rcl/wait.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index c86bdfdc5..1795d575f 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -80,7 +80,7 @@ static void __wait_set_clean_up(rcl_wait_set_t * wait_set, rcl_allocator_t allocator) { if (wait_set->subscriptions) { - rcl_ret_t ret = rcl_wait_set_resize(wait_set, 0u, 0u, 0u, 0u, 0u); + rcl_ret_t ret = rcl_wait_set_resize(wait_set, 0, 0, 0, 0, 0); (void)ret; // NO LINT assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0. } @@ -233,7 +233,7 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al rcl_allocator_t allocator = wait_set->impl->allocator; \ wait_set->size_of_ ## Type ## s = 0; \ wait_set->impl->Type ## _index = 0; \ - if (0u == Type ## s_size) { \ + if (0 == Type ## s_size) { \ if (wait_set->Type ## s) { \ allocator.deallocate((void *)wait_set->Type ## s, allocator.state); \ wait_set->Type ## s = NULL; \ From c71584d0297fa4efa7472be9cad866bc1567d3ab Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Mon, 27 Aug 2018 09:11:53 -0700 Subject: [PATCH 4/5] match order of parameters --- rcl/include/rcl/wait.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index 890b4b275..5eed64f4c 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -266,9 +266,9 @@ rcl_wait_set_clear(rcl_wait_set_t * wait_set); * \param[inout] wait_set struct to be resized * \param[in] subscriptions_size a size for the new subscriptions set * \param[in] guard_conditions_size a size for the new subscriptions set + * \param[in] timers_size a size for the new subscriptions set * \param[in] clients_size a size for the new subscriptions set * \param[in] services_size a size for the new subscriptions set - * \param[in] timers_size a size for the new subscriptions set * \return `RCL_RET_OK` if resized successfully, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or From a1cae4a550dedf173978fdfb846a7519dcdd2f82 Mon Sep 17 00:00:00 2001 From: Dirk Thomas Date: Mon, 27 Aug 2018 09:19:10 -0700 Subject: [PATCH 5/5] swap comparison sides --- rcl/src/rcl/wait.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 1795d575f..2fcd2bbda 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -82,7 +82,7 @@ __wait_set_clean_up(rcl_wait_set_t * wait_set, rcl_allocator_t allocator) if (wait_set->subscriptions) { rcl_ret_t ret = rcl_wait_set_resize(wait_set, 0, 0, 0, 0, 0); (void)ret; // NO LINT - assert(ret == RCL_RET_OK); // Defensive, shouldn't fail with size 0. + assert(RCL_RET_OK == ret); // Defensive, shouldn't fail with size 0. } if (wait_set->impl) { allocator.deallocate(wait_set->impl, allocator.state); @@ -575,7 +575,7 @@ rcl_wait(rcl_wait_set_t * wait_set, int64_t timeout) } } - if (ret == RMW_RET_TIMEOUT && !is_timer_timeout) { + if (RMW_RET_TIMEOUT == ret && !is_timer_timeout) { return RCL_RET_TIMEOUT; } return RCL_RET_OK;