pass context to wait set, and fini rmw context

rcl/include/rcl/wait.h

@@ -85,7 +85,7 @@ rcl_get_zero_initialized_wait_set(void);
  *
  * rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
  * rcl_ret_t ret =
- *   rcl_wait_set_init(&wait_set, 42, 42, 42, 42, 42, rcl_get_default_allocator());
+ *   rcl_wait_set_init(&wait_set, 42, 42, 42, 42, 42, &context, rcl_get_default_allocator());
  * // ... error handling, then use it, then call the matching fini:
  * ret = rcl_wait_set_fini(&wait_set);
  * // ... error handling
@@ -105,9 +105,11 @@ rcl_get_zero_initialized_wait_set(void);
  * \param[in] number_of_timers non-zero size of the timers set
  * \param[in] number_of_clients non-zero size of the clients set
  * \param[in] number_of_services non-zero size of the services set
+ * \param[in] context the context that the wait set should be associated with
  * \param[in] allocator the allocator to use when allocating space in the sets
  * \return `RCL_RET_OK` if the wait set is initialized successfully, or
  * \return `RCL_RET_ALREADY_INIT` if the wait set is not zero initialized, or
+ * \return `RCL_RET_NOT_INIT` if the given context is invalid, or
  * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
  * \return `RCL_RET_BAD_ALLOC` if allocating memory failed, or
  * \return `RCL_RET_ERROR` if an unspecified error occurs.
@@ -122,6 +124,7 @@ rcl_wait_set_init(
   size_t number_of_timers,
   size_t number_of_clients,
   size_t number_of_services,
+  rcl_context_t * context,
   rcl_allocator_t allocator);
 
 /// Finalize a rcl wait set.

rcl/src/rcl/context.c

@@ -126,6 +126,19 @@ __cleanup_context(rcl_context_t * context)
       }
     }
 
+    // clean up rmw_context
+    if (NULL != context->impl->rmw_context.implementation_identifier) {
+      rmw_ret_t rmw_ret = rmw_context_fini(&(context->impl->rmw_context));
+      if (RMW_RET_OK != rmw_ret) {
+        RCUTILS_SAFE_FWRITE_TO_STDERR(
+          "[rcl|init.c:" RCUTILS_STRINGIFY(__LINE__)
+          "] failed to finalize rmw context while cleaning up context, memory may be leaked: ");
+        RCUTILS_SAFE_FWRITE_TO_STDERR(rcutils_get_error_string().str);
+        RCUTILS_SAFE_FWRITE_TO_STDERR("\n");
+        rcutils_reset_error();
+      }
+    }
+
     // clean up copy of argv if valid
     if (NULL != context->impl->argv) {
       int64_t i;

rcl/src/rcl/init.c

@@ -76,6 +76,9 @@ rcl_init(
   RCL_CHECK_FOR_NULL_WITH_MSG(
     context->impl, "failed to allocate memory for context impl", return RCL_RET_BAD_ALLOC);
 
+  // Zero initialize rmw context first so its validity can by checked in cleanup.
+  context->impl->rmw_context = rmw_get_zero_initialized_context();
+
   // Copy the options into the context for future reference.
   rcl_ret_t ret = rcl_init_options_copy(options, &(context->impl->init_options));
   if (RCL_RET_OK != ret) {
@@ -132,7 +135,6 @@ rcl_init(
   context->impl->init_options.impl->rmw_init_options.instance_id = next_instance_id;
 
   // Initialize rmw_init.
-  context->impl->rmw_context = rmw_get_zero_initialized_context();
   rmw_ret_t rmw_ret = rmw_init(
     &(context->impl->init_options.impl->rmw_init_options),
     &(context->impl->rmw_context));

rcl/src/rcl/wait.c

@@ -30,6 +30,8 @@ extern "C"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 
+#include "./context_impl.h"
+
 typedef struct rcl_wait_set_impl_t
 {
   // number of subscriptions that have been added to the wait set
@@ -47,6 +49,9 @@ typedef struct rcl_wait_set_impl_t
   rmw_wait_set_t * rmw_wait_set;
   // number of timers that have been added to the wait set
   size_t timer_index;
+  // context with which the wait set is associated
+  rcl_context_t * context;
+  // allocator used in the wait set
   rcl_allocator_t allocator;
 } rcl_wait_set_impl_t;
 
@@ -97,6 +102,7 @@ rcl_wait_set_init(
   size_t number_of_timers,
   size_t number_of_clients,
   size_t number_of_services,
+  rcl_context_t * context,
   rcl_allocator_t allocator)
 {
   RCUTILS_LOG_DEBUG_NAMED(
@@ -112,6 +118,14 @@ rcl_wait_set_init(
     RCL_SET_ERROR_MSG("wait_set already initialized, or memory was uninitialized.");
     return RCL_RET_ALREADY_INIT;
   }
+  // Make sure rcl has been initialized.
+  RCL_CHECK_ARGUMENT_FOR_NULL(context, RCL_RET_INVALID_ARGUMENT);
+  if (!rcl_context_is_valid(context)) {
+    RCL_SET_ERROR_MSG(
+      "the given context is not valid, "
+      "either rcl_init() was not called or rcl_shutdown() was called.");
+    return RCL_RET_NOT_INIT;
+  }
   // Allocate space for the implementation struct.
   wait_set->impl = (rcl_wait_set_impl_t *)allocator.allocate(
     sizeof(rcl_wait_set_impl_t), allocator.state);
@@ -127,13 +141,19 @@ rcl_wait_set_init(
   wait_set->impl->rmw_services.services = NULL;
   wait_set->impl->rmw_services.service_count = 0;
 
-  wait_set->impl->rmw_wait_set = rmw_create_wait_set(
-    2 * number_of_subscriptions + number_of_guard_conditions + number_of_clients +
-    number_of_services);
+  size_t num_conditions =
+    (2 * number_of_subscriptions) +
+    number_of_guard_conditions +
+    number_of_clients +
+    number_of_services;
+
+  wait_set->impl->rmw_wait_set = rmw_create_wait_set(&(context->impl->rmw_context), num_conditions);
   if (!wait_set->impl->rmw_wait_set) {
     goto fail;
   }
 
+  // Set context.
+  wait_set->impl->context = context;
   // Set allocator.
   wait_set->impl->allocator = allocator;
   // Initialize subscription space.

rcl/test/rcl/client_fixture.cpp

@@ -57,11 +57,12 @@ wait_for_server_to_be_available(
 bool
 wait_for_client_to_be_ready(
   rcl_client_t * client,
+  rcl_context_t * context,
   size_t max_tries,
   int64_t period_ms)
 {
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 1, 0, rcl_get_default_allocator());
+  rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 1, 0, context, rcl_get_default_allocator());
   if (ret != RCL_RET_OK) {
     RCUTILS_LOG_ERROR_NAMED(
       ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string().str);
@@ -207,7 +208,7 @@ int main(int argc, char ** argv)
     memset(&client_response, 0, sizeof(test_msgs__srv__Primitives_Response));
     test_msgs__srv__Primitives_Response__init(&client_response);
 
-    if (!wait_for_client_to_be_ready(&client, 1000, 100)) {
+    if (!wait_for_client_to_be_ready(&client, &context, 1000, 100)) {
       RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Client never became ready");
       return -1;
     }

rcl/test/rcl/service_fixture.cpp

@@ -30,11 +30,12 @@
 bool
 wait_for_service_to_be_ready(
   rcl_service_t * service,
+  rcl_context_t * context,
   size_t max_tries,
   int64_t period_ms)
 {
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, rcl_get_default_allocator());
+  rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, context, rcl_get_default_allocator());
   if (ret != RCL_RET_OK) {
     RCUTILS_LOG_ERROR_NAMED(
       ROS_PACKAGE_NAME, "Error in wait set init: %s", rcl_get_error_string().str);
@@ -156,7 +157,7 @@ int main(int argc, char ** argv)
 
     // Block until a client request comes in.
 
-    if (!wait_for_service_to_be_ready(&service, 1000, 100)) {
+    if (!wait_for_service_to_be_ready(&service, &context, 1000, 100)) {
       RCUTILS_LOG_ERROR_NAMED(ROS_PACKAGE_NAME, "Service never became ready");
       return -1;
     }

rcl/test/rcl/test_count_matched.cpp

@@ -121,7 +121,8 @@ class CLASSNAME (TestCountFixture, RMW_IMPLEMENTATION) : public ::testing::Test
 
     this->wait_set_ptr = new rcl_wait_set_t;
     *this->wait_set_ptr = rcl_get_zero_initialized_wait_set();
-    ret = rcl_wait_set_init(this->wait_set_ptr, 0, 1, 0, 0, 0, rcl_get_default_allocator());
+    ret = rcl_wait_set_init(
+      this->wait_set_ptr, 0, 1, 0, 0, 0, this->context_ptr, rcl_get_default_allocator());
   }
 
   void TearDown()

rcl/test/rcl/test_graph.cpp

@@ -98,7 +98,8 @@ class CLASSNAME (TestGraphFixture, RMW_IMPLEMENTATION) : public ::testing::Test
 
     this->wait_set_ptr = new rcl_wait_set_t;
     *this->wait_set_ptr = rcl_get_zero_initialized_wait_set();
-    ret = rcl_wait_set_init(this->wait_set_ptr, 0, 1, 0, 0, 0, rcl_get_default_allocator());
+    ret = rcl_wait_set_init(
+      this->wait_set_ptr, 0, 1, 0, 0, 0, this->context_ptr, rcl_get_default_allocator());
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   }
 

rcl/test/rcl/test_service.cpp

@@ -78,12 +78,13 @@ class CLASSNAME (TestServiceFixture, RMW_IMPLEMENTATION) : public ::testing::Tes
 void
 wait_for_service_to_be_ready(
   rcl_service_t * service,
+  rcl_context_t * context,
   size_t max_tries,
   int64_t period_ms,
   bool & success)
 {
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, rcl_get_default_allocator());
+  rcl_ret_t ret = rcl_wait_set_init(&wait_set, 0, 0, 0, 0, 1, context, rcl_get_default_allocator());
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
     rcl_ret_t ret = rcl_wait_set_fini(&wait_set);
@@ -174,7 +175,7 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal)
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
 
   bool success;
-  wait_for_service_to_be_ready(&service, 10, 100, success);
+  wait_for_service_to_be_ready(&service, context_ptr, 10, 100, success);
   ASSERT_TRUE(success);
 
   // This scope simulates the service responding in a different context so that we can
@@ -201,7 +202,7 @@ TEST_F(CLASSNAME(TestServiceFixture, RMW_IMPLEMENTATION), test_service_nominal)
     ret = rcl_send_response(&service, &header, &service_response);
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   }
-  wait_for_service_to_be_ready(&service, 10, 100, success);
+  wait_for_service_to_be_ready(&service, context_ptr, 10, 100, success);
 
   // Initialize the response owned by the client and take the response.
   test_msgs__srv__Primitives_Response client_response;

rcl/test/rcl/test_subscription.cpp

@@ -78,12 +78,13 @@ class CLASSNAME (TestSubscriptionFixture, RMW_IMPLEMENTATION) : public ::testing
 void
 wait_for_subscription_to_be_ready(
   rcl_subscription_t * subscription,
+  rcl_context_t * context,
   size_t max_tries,
   int64_t period_ms,
   bool & success)
 {
   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, 0, 0, 0, 0, context, rcl_get_default_allocator());
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   OSRF_TESTING_TOOLS_CPP_SCOPE_EXIT({
     rcl_ret_t ret = rcl_wait_set_fini(&wait_set);
@@ -166,7 +167,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   }
   bool success;
-  wait_for_subscription_to_be_ready(&subscription, 10, 100, success);
+  wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success);
   ASSERT_TRUE(success);
   {
     test_msgs__msg__Primitives msg;
@@ -217,7 +218,7 @@ TEST_F(CLASSNAME(TestSubscriptionFixture, RMW_IMPLEMENTATION), test_subscription
     ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
   }
   bool success;
-  wait_for_subscription_to_be_ready(&subscription, 10, 100, success);
+  wait_for_subscription_to_be_ready(&subscription, context_ptr, 10, 100, success);
   ASSERT_TRUE(success);
   {
     test_msgs__msg__Primitives msg;

rcl/test/rcl/test_timer.cpp

@@ -84,7 +84,7 @@ TEST_F(TestTimerFixture, test_two_timers) {
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
 
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  ret = rcl_wait_set_init(&wait_set, 0, 0, 2, 0, 0, rcl_get_default_allocator());
+  ret = rcl_wait_set_init(&wait_set, 0, 0, 2, 0, 0, context_ptr, rcl_get_default_allocator());
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
 
   ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL);
@@ -140,7 +140,7 @@ TEST_F(TestTimerFixture, test_two_timers_ready_before_timeout) {
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
 
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  ret = rcl_wait_set_init(&wait_set, 0, 0, 2, 0, 0, rcl_get_default_allocator());
+  ret = rcl_wait_set_init(&wait_set, 0, 0, 2, 0, 0, context_ptr, rcl_get_default_allocator());
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
 
   ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL);
@@ -191,7 +191,7 @@ TEST_F(TestTimerFixture, test_timer_not_ready) {
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
 
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, rcl_get_default_allocator());
+  ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, context_ptr, rcl_get_default_allocator());
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
 
   ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL);
@@ -239,7 +239,7 @@ TEST_F(TestTimerFixture, test_canceled_timer) {
   ASSERT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
 
   rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-  ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, rcl_get_default_allocator());
+  ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, context_ptr, rcl_get_default_allocator());
   EXPECT_EQ(RCL_RET_OK, ret) << rcl_get_error_string().str;
 
   ret = rcl_wait_set_add_timer(&wait_set, &timer, NULL);
@@ -459,7 +459,7 @@ TEST_F(TestTimerFixture, test_ros_time_wakes_wait) {
 
   std::thread wait_thr([&](void) {
       rcl_wait_set_t wait_set = rcl_get_zero_initialized_wait_set();
-      ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, rcl_get_default_allocator());
+      ret = rcl_wait_set_init(&wait_set, 0, 0, 1, 0, 0, context_ptr, 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, NULL)) <<