Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[rcl] Output index in container when adding an entity to a wait set #335

Merged
merged 2 commits into from
Nov 20, 2018
Merged

[rcl] Output index in container when adding an entity to a wait set #335

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
53ed8a7
[rcl] Output index in container when adding an entity to a wait set
jacobperron Nov 19, 2018
39dfeed
Refactor actions wait set functions to leverage new wait set API
jacobperron Nov 19, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 12 additions & 5 deletions rcl/include/rcl/wait.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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.
/**
Expand Down Expand Up @@ -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.
/**
Expand All @@ -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.
/**
Expand All @@ -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.
/**
Expand All @@ -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.
/**
Expand Down
22 changes: 16 additions & 6 deletions rcl/src/rcl/wait.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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. */ \
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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.
Expand All @@ -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)
Expand All @@ -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)
Expand Down
2 changes: 1 addition & 1 deletion rcl/test/rcl/client_fixture.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 1 addition & 1 deletion rcl/test/rcl/service_fixture.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
6 changes: 3 additions & 3 deletions rcl/test/rcl/test_graph.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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",
Expand Down
2 changes: 1 addition & 1 deletion rcl/test/rcl/test_service.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
2 changes: 1 addition & 1 deletion rcl/test/rcl/test_subscription.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
14 changes: 7 additions & 7 deletions rcl/test/rcl/test_timer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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({
Expand Down Expand Up @@ -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({
Expand Down Expand Up @@ -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({
Expand Down
44 changes: 35 additions & 9 deletions rcl/test/rcl/test_wait.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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({
Expand Down Expand Up @@ -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({
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand All @@ -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({
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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({
Expand Down Expand Up @@ -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]);
}
}
Loading