From 87d41f08082635d26a50fec52d698533e096b7be Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 19 Nov 2018 11:14:41 -0800 Subject: [PATCH] [rcl] Output index in container when adding an entity to a wait set This feature is useful when different libraries interact with the same wait set (e.g. rcl_action). --- rcl/include/rcl/wait.h | 17 ++++++++---- rcl/src/rcl/wait.c | 22 +++++++++++---- rcl/test/rcl/client_fixture.cpp | 2 +- rcl/test/rcl/service_fixture.cpp | 2 +- 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_timer.cpp | 14 +++++----- rcl/test/rcl/test_wait.cpp | 44 ++++++++++++++++++++++++------ 9 files changed, 77 insertions(+), 34 deletions(-) diff --git a/rcl/include/rcl/wait.h b/rcl/include/rcl/wait.h index 05265d591..4e7673dd2 100644 --- a/rcl/include/rcl/wait.h +++ b/rcl/include/rcl/wait.h @@ -196,6 +196,8 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al * * \param[inout] wait_set struct in which the subscription is to be stored * \param[in] subscription the subscription to be added to the wait set + * \param[out] index the index of the added subscription in the storage container. + * This parameter is optional and can be set to `NULL` to be ignored. * \return `RCL_RET_OK` if added 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 @@ -207,7 +209,8 @@ RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_add_subscription( rcl_wait_set_t * wait_set, - const rcl_subscription_t * subscription); + const rcl_subscription_t * subscription, + size_t * index); /// Remove (sets to `NULL`) all entities in the wait set. /** @@ -295,7 +298,8 @@ RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_add_guard_condition( rcl_wait_set_t * wait_set, - const rcl_guard_condition_t * guard_condition); + const rcl_guard_condition_t * guard_condition, + size_t * index); /// Store a pointer to the timer in the next empty spot in the set. /** @@ -307,7 +311,8 @@ RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_add_timer( rcl_wait_set_t * wait_set, - const rcl_timer_t * timer); + const rcl_timer_t * timer, + size_t * index); /// Store a pointer to the client in the next empty spot in the set. /** @@ -319,7 +324,8 @@ RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_add_client( rcl_wait_set_t * wait_set, - const rcl_client_t * client); + const rcl_client_t * client, + size_t * index); /// Store a pointer to the service in the next empty spot in the set. /** @@ -331,7 +337,8 @@ RCL_WARN_UNUSED rcl_ret_t rcl_wait_set_add_service( rcl_wait_set_t * wait_set, - const rcl_service_t * service); + const rcl_service_t * service, + size_t * index); /// Block until the wait set is ready or until the timeout has been exceeded. /** diff --git a/rcl/src/rcl/wait.c b/rcl/src/rcl/wait.c index 680a19abb..76a1098bc 100644 --- a/rcl/src/rcl/wait.c +++ b/rcl/src/rcl/wait.c @@ -198,7 +198,11 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al return RCL_RET_WAIT_SET_FULL; \ } \ size_t current_index = wait_set->impl->Type ## _index++; \ - wait_set->Type ## s[current_index] = Type; + wait_set->Type ## s[current_index] = Type; \ + /* Set optional output argument */ \ + if (NULL != index) { \ + *index = current_index; \ + } #define SET_ADD_RMW(Type, RMWStorage, RMWCount) \ /* Also place into rmw storage. */ \ @@ -283,7 +287,8 @@ rcl_wait_set_get_allocator(const rcl_wait_set_t * wait_set, rcl_allocator_t * al rcl_ret_t rcl_wait_set_add_subscription( rcl_wait_set_t * wait_set, - const rcl_subscription_t * subscription) + const rcl_subscription_t * subscription, + size_t * index) { SET_ADD(subscription) SET_ADD_RMW(subscription, rmw_subscriptions.subscribers, rmw_subscriptions.subscriber_count) @@ -402,18 +407,21 @@ rcl_wait_set_resize( rcl_ret_t rcl_wait_set_add_guard_condition( rcl_wait_set_t * wait_set, - const rcl_guard_condition_t * guard_condition) + const rcl_guard_condition_t * guard_condition, + size_t * index) { SET_ADD(guard_condition) SET_ADD_RMW(guard_condition, rmw_guard_conditions.guard_conditions, rmw_guard_conditions.guard_condition_count) + return RCL_RET_OK; } rcl_ret_t rcl_wait_set_add_timer( rcl_wait_set_t * wait_set, - const rcl_timer_t * timer) + const rcl_timer_t * timer, + size_t * index) { SET_ADD(timer) // Add timer guard conditions to end of rmw guard condtion set. @@ -432,7 +440,8 @@ rcl_wait_set_add_timer( rcl_ret_t rcl_wait_set_add_client( rcl_wait_set_t * wait_set, - const rcl_client_t * client) + const rcl_client_t * client, + size_t * index) { SET_ADD(client) SET_ADD_RMW(client, rmw_clients.clients, rmw_clients.client_count) @@ -442,7 +451,8 @@ rcl_wait_set_add_client( rcl_ret_t rcl_wait_set_add_service( rcl_wait_set_t * wait_set, - const rcl_service_t * service) + const rcl_service_t * service, + size_t * index) { SET_ADD(service) SET_ADD_RMW(service, rmw_services.services, rmw_services.service_count) diff --git a/rcl/test/rcl/client_fixture.cpp b/rcl/test/rcl/client_fixture.cpp index 8af8b27cc..f0a56dab7 100644 --- a/rcl/test/rcl/client_fixture.cpp +++ b/rcl/test/rcl/client_fixture.cpp @@ -82,7 +82,7 @@ wait_for_client_to_be_ready( 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) != RCL_RET_OK) { + 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; diff --git a/rcl/test/rcl/service_fixture.cpp b/rcl/test/rcl/service_fixture.cpp index 437168eff..58d128064 100644 --- a/rcl/test/rcl/service_fixture.cpp +++ b/rcl/test/rcl/service_fixture.cpp @@ -55,7 +55,7 @@ wait_for_service_to_be_ready( 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) != RCL_RET_OK) { + 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; diff --git a/rcl/test/rcl/test_graph.cpp b/rcl/test/rcl/test_graph.cpp index 0c347b8fc..fcddf7768 100644 --- a/rcl/test/rcl/test_graph.cpp +++ b/rcl/test/rcl/test_graph.cpp @@ -287,7 +287,7 @@ check_graph_state( } ret = rcl_wait_set_clear(wait_set_ptr); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_guard_condition(wait_set_ptr, graph_guard_condition); + ret = rcl_wait_set_add_guard_condition(wait_set_ptr, graph_guard_condition, NULL); ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200); RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, @@ -446,7 +446,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_graph_guard_conditi 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(this->wait_set_ptr, graph_guard_condition); + 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; std::chrono::nanoseconds time_to_sleep = std::chrono::milliseconds(200); RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, @@ -504,7 +504,7 @@ TEST_F(CLASSNAME(TestGraphFixture, RMW_IMPLEMENTATION), test_rcl_service_server_ } rcl_ret_t 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(this->wait_set_ptr, graph_guard_condition); + 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; RCUTILS_LOG_INFO_NAMED(ROS_PACKAGE_NAME, "waiting up to '%s' nanoseconds for graph changes", diff --git a/rcl/test/rcl/test_service.cpp b/rcl/test/rcl/test_service.cpp index b54fa6242..9f4459846 100644 --- a/rcl/test/rcl/test_service.cpp +++ b/rcl/test/rcl/test_service.cpp @@ -80,7 +80,7 @@ wait_for_service_to_be_ready( ++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); + 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) { diff --git a/rcl/test/rcl/test_subscription.cpp b/rcl/test/rcl/test_subscription.cpp index af563732d..d42faaaef 100644 --- a/rcl/test/rcl/test_subscription.cpp +++ b/rcl/test/rcl/test_subscription.cpp @@ -80,7 +80,7 @@ wait_for_subscription_to_be_ready( ++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); + 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) { diff --git a/rcl/test/rcl/test_timer.cpp b/rcl/test/rcl/test_timer.cpp index 98d65cd0f..5248142f6 100644 --- a/rcl/test/rcl/test_timer.cpp +++ b/rcl/test/rcl/test_timer.cpp @@ -71,9 +71,9 @@ TEST_F(TestTimerFixture, test_two_timers) { ret = rcl_wait_set_init(&wait_set, 0, 0, 2, 0, 0, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_timer(&wait_set, &timer); + ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_timer(&wait_set, &timer2); + ret = rcl_wait_set_add_timer(&wait_set, &timer2, NULL); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ rcl_ret_t ret = rcl_timer_fini(&timer); @@ -125,9 +125,9 @@ TEST_F(TestTimerFixture, test_two_timers_ready_before_timeout) { ret = rcl_wait_set_init(&wait_set, 0, 0, 2, 0, 0, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_timer(&wait_set, &timer); + ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_timer(&wait_set, &timer2); + ret = rcl_wait_set_add_timer(&wait_set, &timer2, NULL); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ rcl_ret_t ret = rcl_timer_fini(&timer); @@ -175,7 +175,7 @@ TEST_F(TestTimerFixture, test_timer_not_ready) { ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_timer(&wait_set, &timer); + ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ @@ -222,7 +222,7 @@ TEST_F(TestTimerFixture, test_canceled_timer) { ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_timer(&wait_set, &timer); + ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ @@ -437,7 +437,7 @@ TEST_F(TestTimerFixture, test_ros_time_wakes_wait) { ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ASSERT_EQ(RCL_RET_OK, rcl_wait_set_add_timer(&wait_set, &timer)) << + ASSERT_EQ(RCL_RET_OK, rcl_wait_set_add_timer(&wait_set, &timer, NULL)) << rcl_get_error_string().str; // *INDENT-OFF* (Uncrustify wants strange un-indentation here) OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ diff --git a/rcl/test/rcl/test_wait.cpp b/rcl/test/rcl/test_wait.cpp index 201cf7f31..b252e1038 100644 --- a/rcl/test/rcl/test_wait.cpp +++ b/rcl/test/rcl/test_wait.cpp @@ -104,7 +104,7 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), negative_timeout) { rcl_guard_condition_t guard_cond = rcl_get_zero_initialized_guard_condition(); ret = rcl_guard_condition_init(&guard_cond, rcl_guard_condition_get_default_options()); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond); + ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond, NULL); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_clock_t clock; @@ -115,7 +115,7 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), negative_timeout) { rcl_timer_t timer = rcl_get_zero_initialized_timer(); ret = rcl_timer_init(&timer, &clock, RCL_MS_TO_NS(10), nullptr, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_timer(&wait_set, &timer); + ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ @@ -148,7 +148,7 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), zero_timeout) { rcl_guard_condition_t guard_cond = rcl_get_zero_initialized_guard_condition(); ret = rcl_guard_condition_init(&guard_cond, rcl_guard_condition_get_default_options()); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond); + ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond, NULL); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ @@ -178,7 +178,7 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), zero_timeout_triggered rcl_guard_condition_t guard_cond = rcl_get_zero_initialized_guard_condition(); ret = rcl_guard_condition_init(&guard_cond, rcl_guard_condition_get_default_options()); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond); + ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond, NULL); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_trigger_guard_condition(&guard_cond); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; @@ -211,7 +211,7 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), canceled_timer) { rcl_guard_condition_t guard_cond = rcl_get_zero_initialized_guard_condition(); ret = rcl_guard_condition_init(&guard_cond, rcl_guard_condition_get_default_options()); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond); + ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond, NULL); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; rcl_clock_t clock; @@ -225,7 +225,7 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), canceled_timer) { EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_timer_cancel(&canceled_timer); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_timer(&wait_set, &canceled_timer); + ret = rcl_wait_set_add_timer(&wait_set, &canceled_timer, NULL); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ @@ -293,7 +293,8 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), multi_wait_set_threade rcl_ret_t ret; ret = rcl_wait_set_clear(&test_set.wait_set); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_guard_condition(&test_set.wait_set, &test_set.guard_condition); + ret = rcl_wait_set_add_guard_condition( + &test_set.wait_set, &test_set.guard_condition, NULL); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; ret = rcl_wait(&test_set.wait_set, wait_period); if (ret != RCL_RET_TIMEOUT) { @@ -339,7 +340,7 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), multi_wait_set_threade test_set.wait_set = rcl_get_zero_initialized_wait_set(); ret = rcl_wait_set_init(&test_set.wait_set, 0, 1, 0, 0, 0, rcl_get_default_allocator()); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_guard_condition(&test_set.wait_set, &test_set.guard_condition); + ret = rcl_wait_set_add_guard_condition(&test_set.wait_set, &test_set.guard_condition, NULL); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; test_set.thread_id = 0; } @@ -399,7 +400,7 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), guard_condition) { rcl_guard_condition_t guard_cond = rcl_get_zero_initialized_guard_condition(); ret = rcl_guard_condition_init(&guard_cond, rcl_guard_condition_get_default_options()); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; - ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond); + ret = rcl_wait_set_add_guard_condition(&wait_set, &guard_cond, NULL); EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({ @@ -435,3 +436,28 @@ TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), guard_condition) { EXPECT_EQ(RCL_RET_OK, f.get()); EXPECT_LE(std::abs(diff - trigger_diff.count()), TOLERANCE); } + +// Check that index arguments are properly set when adding entities +TEST_F(CLASSNAME(WaitSetTestFixture, RMW_IMPLEMENTATION), add_with_index) { + const size_t kNumEntities = 3u; + rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set(); + rcl_ret_t ret = rcl_wait_set_init( + &wait_set, 0, kNumEntities, 0, 0, 0, rcl_get_default_allocator()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + + // Initialize to invalid value + size_t guard_condition_index = 42u; + + rcl_guard_condition_t guard_conditions[kNumEntities]; + for (size_t i = 0u; i < kNumEntities; ++i) { + guard_conditions[i] = rcl_get_zero_initialized_guard_condition(); + ret = rcl_guard_condition_init( + &guard_conditions[i], rcl_guard_condition_get_default_options()); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + ret = rcl_wait_set_add_guard_condition( + &wait_set, &guard_conditions[i], &guard_condition_index); + EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str; + EXPECT_EQ(guard_condition_index, i); + EXPECT_EQ(&guard_conditions[i], wait_set.guard_conditions[i]); + } +}