From 53ed8a71e3a84dd5070415d50979c41a5d75466f Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 19 Nov 2018 11:14:41 -0800 Subject: [PATCH 1/2] [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]); + } +} From 39dfeed2c2adcd429496669deceb8359e4f3cf9d Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Mon, 19 Nov 2018 15:10:59 -0800 Subject: [PATCH 2/2] Refactor actions wait set functions to leverage new wait set API --- rcl_action/include/rcl_action/wait.h | 34 ++++++-- rcl_action/src/rcl_action/action_client.c | 95 +++++++++++++++++++---- 2 files changed, 106 insertions(+), 23 deletions(-) diff --git a/rcl_action/include/rcl_action/wait.h b/rcl_action/include/rcl_action/wait.h index 37d13d2cf..f3b0d7c24 100644 --- a/rcl_action/include/rcl_action/wait.h +++ b/rcl_action/include/rcl_action/wait.h @@ -30,13 +30,17 @@ extern "C" * This function will add the underlying service clients and subscribers to the wait set. * * This function behaves similar to adding subscriptions to the wait set, but will add - * five elements: + * five entities: * * - Three service clients - * - Two subscribers + * - Two subscriptions * * \see rcl_wait_set_add_subscription * + * If this function fails for any reason, `client_index` and `subscription_index` are not set. + * It is also possible the provided wait set is left in an inconsistent state (e.g. some + * of the clients and subscriptions were added to the wait set, but not all). + * *
* Attribute | Adherence * ------------------ | ------------- @@ -48,6 +52,12 @@ extern "C" * \param[inout] wait_set struct where action client service client and subscription * are to be stored * \param[in] action_client the action client to be added to the wait set + * \param[out] client_index the starting index in the wait set's client container where + * the action clients underlying service clients were added. Optionally, set to `NULL` + * if ignored. + * \param[out] subscription_index the starting index in the wait set's subscription container + * where the action clients underlying subscriptions were added. Optionally, set to `NULL` + * if ignored. * \return `RCL_RET_OK` if added successfully, or * \return `RCL_RET_WAIT_SET_INVALID` if the wait set is zero initialized, or * \return `RCL_RET_WAIT_SET_FULL` if the subscription set is full, or @@ -59,7 +69,9 @@ RCL_WARN_UNUSED rcl_ret_t rcl_action_wait_set_add_action_client( rcl_wait_set_t * wait_set, - const rcl_action_client_t * action_client); + const rcl_action_client_t * action_client, + size_t * client_index, + size_t * subscription_index); /// Add a rcl_action_server_t to a wait set. /** @@ -70,6 +82,10 @@ rcl_action_wait_set_add_action_client( * * \see rcl_wait_set_add_service * + * * If this function fails for any reason, `service_index` is not set. + * It is also possible the provided wait set is left in an inconsistent state (e.g. some + * of the clients and subscribers were added to the wait set, but not all). + *
* Attribute | Adherence * ------------------ | ------------- @@ -80,6 +96,9 @@ rcl_action_wait_set_add_action_client( * * \param[inout] wait_set struct where action server services are to be stored * \param[in] action_server the action server to be added to the wait set + * \param[out] service_index the starting index in the wait set's service container where + * the action servers underlying services were added. Optionally, set to `NULL` + * if ignored. * \return `RCL_RET_OK` if added successfully, or * \return `RCL_RET_WAIT_SET_INVALID` if the wait set is zero initialized, or * \return `RCL_RET_WAIT_SET_FULL` if the subscription set is full, or @@ -91,7 +110,8 @@ RCL_WARN_UNUSED rcl_ret_t rcl_action_wait_set_add_action_server( rcl_wait_set_t * wait_set, - const rcl_action_server_t * action_server); + const rcl_action_server_t * action_server, + size_t * service_index); /// Get the number of wait set entities associated with a rcl_action_client_t. /** @@ -179,6 +199,7 @@ rcl_action_server_wait_set_get_num_entities( * to call: rcl_action_take_feedback(), rcl_action_take_status(), * rcl_action_take_goal_response(), rcl_action_take_cancel_response(), or * rcl_action_take_result_response(). + * *
* Attribute | Adherence * ------------------ | ------------- @@ -199,8 +220,7 @@ rcl_action_server_wait_set_get_num_entities( * \param[out] is_result_response_ready `true` if there is a result response message ready * to take, `false` otherwise * \return `RCL_RET_OK` if call is successful, or - * \return `RCL_RET_WAIT_SET_INVALID` if the wait set is zero initialized or not used - * for the action client alone, or + * \return `RCL_RET_WAIT_SET_INVALID` if the wait set is invalid, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_ACTION_CLIENT_INVALID` if the action client is invalid, or * \return `RCL_RET_ERROR` if an unspecified error occurs. @@ -222,6 +242,7 @@ rcl_action_client_wait_set_get_entities_ready( * The caller can use this function to determine the relevant action server functions * to call: rcl_action_take_goal_request(), rcl_action_take_cancel_request(), or * rcl_action_take_result_request(). + * *
* Attribute | Adherence * ------------------ | ------------- @@ -238,6 +259,7 @@ rcl_action_client_wait_set_get_entities_ready( * \param[out] is_result_request_ready `true` if there is a result request message ready * to take, `false` otherwise * \return `RCL_RET_OK` if call is successful, or + * \return `RCL_RET_WAIT_SET_INVALID` if the wait set is invalid, or * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or * \return `RCL_RET_ACTION_CLIENT_INVALID` if the action server is invalid, or * \return `RCL_RET_ERROR` if an unspecified error occurs. diff --git a/rcl_action/src/rcl_action/action_client.c b/rcl_action/src/rcl_action/action_client.c index 48d4049ae..1e02a99c3 100644 --- a/rcl_action/src/rcl_action/action_client.c +++ b/rcl_action/src/rcl_action/action_client.c @@ -46,6 +46,12 @@ typedef struct rcl_action_client_impl_t rcl_subscription_t status_subscription; rcl_action_client_options_t options; char * action_name; + // Wait set records + size_t wait_set_goal_client_index; + size_t wait_set_cancel_client_index; + size_t wait_set_result_client_index; + size_t wait_set_feedback_subscription_index; + size_t wait_set_status_subscription_index; } rcl_action_client_impl_t; rcl_action_client_t @@ -409,39 +415,65 @@ rcl_action_client_is_valid(const rcl_action_client_t * action_client) rcl_ret_t rcl_action_wait_set_add_action_client( rcl_wait_set_t * wait_set, - const rcl_action_client_t * action_client) + const rcl_action_client_t * action_client, + size_t * client_index, + size_t * subscription_index) { rcl_ret_t ret; RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_WAIT_SET_INVALID); if (!rcl_action_client_is_valid(action_client)) { return RCL_RET_ACTION_CLIENT_INVALID; // error already set } + // Wait on action goal service response messages. - ret = rcl_wait_set_add_client(wait_set, &action_client->impl->goal_client); + ret = rcl_wait_set_add_client( + wait_set, + &action_client->impl->goal_client, + &action_client->impl->wait_set_goal_client_index); if (RCL_RET_OK != ret) { return ret; } // Wait on action cancel service response messages. - ret = rcl_wait_set_add_client(wait_set, &action_client->impl->cancel_client); + ret = rcl_wait_set_add_client( + wait_set, + &action_client->impl->cancel_client, + &action_client->impl->wait_set_cancel_client_index); if (RCL_RET_OK != ret) { return ret; } // Wait on action result service response messages. - ret = rcl_wait_set_add_client(wait_set, &action_client->impl->result_client); + ret = rcl_wait_set_add_client( + wait_set, + &action_client->impl->result_client, + &action_client->impl->wait_set_result_client_index); if (RCL_RET_OK != ret) { return ret; } // Wait on action feedback messages. - ret = rcl_wait_set_add_subscription(wait_set, &action_client->impl->feedback_subscription); + ret = rcl_wait_set_add_subscription( + wait_set, + &action_client->impl->feedback_subscription, + &action_client->impl->wait_set_feedback_subscription_index); if (RCL_RET_OK != ret) { return ret; } - return RCL_RET_OK; // Wait on action status messages. - ret = rcl_wait_set_add_subscription(wait_set, &action_client->impl->status_subscription); + ret = rcl_wait_set_add_subscription( + wait_set, + &action_client->impl->status_subscription, + &action_client->impl->wait_set_status_subscription_index); if (RCL_RET_OK != ret) { return ret; } + + if (NULL != client_index) { + // The goal client was the first added + *client_index = action_client->impl->wait_set_goal_client_index; + } + if (NULL != subscription_index) { + // The feedback subscription was the first added + *subscription_index = action_client->impl->wait_set_feedback_subscription_index; + } return RCL_RET_OK; } @@ -480,6 +512,7 @@ rcl_action_client_wait_set_get_entities_ready( bool * is_cancel_response_ready, bool * is_result_response_ready) { + RCL_CHECK_ARGUMENT_FOR_NULL(wait_set, RCL_RET_WAIT_SET_INVALID); if (!rcl_action_client_is_valid(action_client)) { return RCL_RET_ACTION_CLIENT_INVALID; // error already set } @@ -488,19 +521,47 @@ rcl_action_client_wait_set_get_entities_ready( RCL_CHECK_ARGUMENT_FOR_NULL(is_goal_response_ready, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(is_cancel_response_ready, RCL_RET_INVALID_ARGUMENT); RCL_CHECK_ARGUMENT_FOR_NULL(is_result_response_ready, RCL_RET_INVALID_ARGUMENT); - if (2 != wait_set->size_of_subscriptions || 3 != wait_set->size_of_clients) { - RCL_SET_ERROR_MSG("wait set not initialized or not used by the action client alone"); - return RCL_RET_WAIT_SET_INVALID; - } - *is_feedback_ready = !!wait_set->subscriptions[0]; - *is_status_ready = !!wait_set->subscriptions[1]; - *is_goal_response_ready = !!wait_set->clients[0]; - *is_cancel_response_ready = !!wait_set->clients[1]; - *is_result_response_ready = !!wait_set->clients[2]; + + const rcl_action_client_impl_t * impl = action_client->impl; + const size_t feedback_index = impl->wait_set_feedback_subscription_index; + const size_t status_index = impl->wait_set_status_subscription_index; + const size_t goal_index = impl->wait_set_goal_client_index; + const size_t cancel_index = impl->wait_set_cancel_client_index; + const size_t result_index = impl->wait_set_result_client_index; + if (feedback_index >= wait_set->size_of_subscriptions) { + RCL_SET_ERROR_MSG("wait set index for feedback subscription is out of bounds"); + return RCL_RET_ERROR; + } + if (status_index >= wait_set->size_of_subscriptions) { + RCL_SET_ERROR_MSG("wait set index for status subscription is out of bounds"); + return RCL_RET_ERROR; + } + if (goal_index >= wait_set->size_of_clients) { + RCL_SET_ERROR_MSG("wait set index for goal client is out of bounds"); + return RCL_RET_ERROR; + } + if (cancel_index >= wait_set->size_of_clients) { + RCL_SET_ERROR_MSG("wait set index for cancel client is out of bounds"); + return RCL_RET_ERROR; + } + if (result_index >= wait_set->size_of_clients) { + RCL_SET_ERROR_MSG("wait set index for result client is out of bounds"); + return RCL_RET_ERROR; + } + + const rcl_subscription_t * feedback_subscription = wait_set->subscriptions[feedback_index]; + const rcl_subscription_t * status_subscription = wait_set->subscriptions[status_index]; + const rcl_client_t * goal_client = wait_set->clients[goal_index]; + const rcl_client_t * cancel_client = wait_set->clients[cancel_index]; + const rcl_client_t * result_client = wait_set->clients[result_index]; + *is_feedback_ready = (&impl->feedback_subscription == feedback_subscription); + *is_status_ready = (&impl->status_subscription == status_subscription); + *is_goal_response_ready = (&impl->goal_client == goal_client); + *is_cancel_response_ready = (&impl->cancel_client == cancel_client); + *is_result_response_ready = (&impl->result_client == result_client); return RCL_RET_OK; } - #ifdef __cplusplus } #endif