[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

@@ -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.
 /**

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)

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;

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;

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",

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) {

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) {

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({

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]);
+  }
+}