Updates

rmw_microxrcedds_c/src/memory.c

@@ -105,9 +105,8 @@ void put_memory(struct rmw_uxrce_mempool_t * mem, struct rmw_uxrce_mempool_item_
     mem->allocateditems = item->next;
   }
 
-  // if (mem->freeitems)
-  // {
-  //     mem->freeitems->prev    = NULL;
+  // if (mem->freeitems){
+  //     mem->freeitems->prev = NULL;
   // }
 
   // Puts item in free pool

rmw_microxrcedds_c/src/rmw_client.c

@@ -69,6 +69,8 @@ rmw_create_client(
     }
 
     rmw_uxrce_client_t * custom_client = (rmw_uxrce_client_t *)memory_node->data;
+    custom_client->rmw_handle = rmw_client;
+
     custom_client->owner_node = custom_node;
     custom_client->client_gid.implementation_identifier =
       rmw_get_implementation_identifier();

rmw_microxrcedds_c/src/rmw_init.c

@@ -328,9 +328,21 @@ rmw_ret_t
 rmw_context_fini(rmw_context_t * context)
 {
   // TODO(pablogs9): Should we manage not closed XRCE sessions?
+  rmw_ret_t ret = RMW_RET_OK;
+
+  struct rmw_uxrce_mempool_item_t * item = node_memory.allocateditems;
+
+  while (item != NULL) {
+    rmw_uxrce_node_t * custom_node = (rmw_uxrce_node_t *)item->data;
+    item = item->next;
+    if (custom_node->context == context->impl) {
+      ret = rmw_destroy_node(custom_node->rmw_handle);
+    }
+  }
+
   uxr_delete_session(&context->impl->session);
   rmw_uxrce_fini_session_memory(context->impl);
   context->impl = NULL;
 
-  return RMW_RET_OK;
+  return ret;
 }

rmw_microxrcedds_c/src/rmw_node.c

@@ -35,6 +35,8 @@ rmw_node_t * create_node(
   const char * name, const char * namespace_, size_t domain_id,
   const rmw_context_t * context)
 {
+  rmw_node_t * node_handle = NULL;
+
   if (!context) {
     RMW_SET_ERROR_MSG("context is null");
     return NULL;
@@ -43,19 +45,21 @@ rmw_node_t * create_node(
   struct rmw_uxrce_mempool_item_t * memory_node = get_memory(&node_memory);
   if (!memory_node) {
     RMW_SET_ERROR_MSG("Not available memory node");
-    return NULL;
+    goto fail;
   }
 
   rmw_uxrce_node_t * node_info = (rmw_uxrce_node_t *)memory_node->data;
 
   node_info->context = context->impl;
 
-  rmw_node_t * node_handle = NULL;
   node_handle = rmw_node_allocate();
   if (!node_handle) {
     RMW_SET_ERROR_MSG("failed to allocate rmw_node_t");
     return NULL;
   }
+
+  node_info->rmw_handle = node_handle;
+
   node_handle->implementation_identifier = rmw_get_implementation_identifier();
   node_handle->data = node_info;
   node_handle->name = (const char *)(rmw_allocate(sizeof(char) * (strlen(name) + 1)));
@@ -105,13 +109,19 @@ rmw_node_t * create_node(
   uint16_t requests[] = {participant_req};
 
   if (!uxr_run_session_until_all_status(&node_info->context->session, 1000, requests, status, 1)) {
-    uxr_delete_session(&node_info->context->session);
     rmw_uxrce_fini_node_memory(node_handle);
     RMW_SET_ERROR_MSG("Issues creating micro XRCE-DDS entities");
     return NULL;
   }
 
   return node_handle;
+
+fail:
+  if (node_handle != NULL) {
+    rmw_uxrce_fini_node_memory(node_handle);
+  }
+  node_handle = NULL;
+  return node_handle;
 }
 
 rmw_node_t *
@@ -156,7 +166,6 @@ rmw_ret_t rmw_destroy_node(rmw_node_t * node)
   }
 
   rmw_uxrce_node_t * custom_node = (rmw_uxrce_node_t *)node->data;
-  // TODO(Borja) make sure that session deletion deletes participant and related entities.
   // TODO(Pablo) make sure that other entities are removed from the pools
 
   struct rmw_uxrce_mempool_item_t * item = NULL;
@@ -197,6 +206,20 @@ rmw_ret_t rmw_destroy_node(rmw_node_t * node)
     }
   }
 
+  uint16_t participant_req = uxr_buffer_delete_entity(
+    &custom_node->context->session,
+    custom_node->context->reliable_output,
+    custom_node->participant_id);
+  uint8_t status[1];
+  uint16_t requests[] = {participant_req};
+
+  if (!uxr_run_session_until_all_status(
+      &custom_node->context->session, 1000, requests, status,
+      1))
+  {
+    ret = RMW_RET_ERROR;
+  }
+
   rmw_uxrce_fini_node_memory(node);
 
   return ret;

rmw_microxrcedds_c/src/rmw_publisher.c

@@ -93,6 +93,7 @@ rmw_create_publisher(
 
     // TODO(Borja) micro_xrcedds_id is duplicated in publisher_id and in publisher_gid.data
     rmw_uxrce_publisher_t * custom_publisher = (rmw_uxrce_publisher_t *)memory_node->data;
+    custom_publisher->rmw_handle = rmw_publisher;
     custom_publisher->owner_node = custom_node;
     custom_publisher->publisher_gid.implementation_identifier = rmw_get_implementation_identifier();
     memcpy(&custom_publisher->qos, qos_policies, sizeof(rmw_qos_profile_t));

rmw_microxrcedds_c/src/rmw_service.c

@@ -68,6 +68,8 @@ rmw_create_service(
     }
 
     rmw_uxrce_service_t * custom_service = (rmw_uxrce_service_t *)memory_node->data;
+    custom_service->rmw_handle = rmw_service;
+
     custom_service->owner_node = custom_node;
     custom_service->service_gid.implementation_identifier =
       rmw_get_implementation_identifier();

rmw_microxrcedds_c/src/rmw_subscription.c

@@ -93,6 +93,8 @@ rmw_create_subscription(
 
     // TODO(Borja) micro_xrcedds_id is duplicated in subscriber_id and in subscription_gid.data
     rmw_uxrce_subscription_t * custom_subscription = (rmw_uxrce_subscription_t *)memory_node->data;
+    custom_subscription->rmw_handle = rmw_subscription;
+
     custom_subscription->owner_node = custom_node;
     custom_subscription->subscription_gid.implementation_identifier =
       rmw_get_implementation_identifier();

rmw_microxrcedds_c/src/types.c

@@ -204,9 +204,11 @@ void rmw_uxrce_fini_node_memory(rmw_node_t * node)
   }
   if (node->data) {
     rmw_uxrce_node_t * custom_node = (rmw_uxrce_node_t *)node->data;
+    custom_node->rmw_handle = NULL;
+    custom_node->context = NULL;
+
     put_memory(&node_memory, &custom_node->mem);
 
-    memset(custom_node, 0, sizeof(rmw_uxrce_node_t));
     node->data = NULL;
   }
 
@@ -232,10 +234,9 @@ void rmw_uxrce_fini_publisher_memory(rmw_publisher_t * publisher)
     if (custom_publisher->topic != NULL) {
       rmw_uxrce_fini_topic_memory(custom_publisher->topic);
     }
+    custom_publisher->rmw_handle = NULL;
 
     put_memory(&publisher_memory, &custom_publisher->mem);
-
-    memset(custom_publisher, 0, sizeof(rmw_uxrce_publisher_t));
     publisher->data = NULL;
   }
 
@@ -260,10 +261,9 @@ void rmw_uxrce_fini_subscription_memory(rmw_subscription_t * subscriber)
     if (custom_subscription->topic != NULL) {
       rmw_uxrce_fini_topic_memory(custom_subscription->topic);
     }
+    custom_subscription->rmw_handle = NULL;
 
     put_memory(&subscription_memory, &custom_subscription->mem);
-
-    memset(custom_subscription, 0, sizeof(rmw_uxrce_subscription_t));
     subscriber->data = NULL;
   }
   rmw_free(subscriber);
@@ -283,10 +283,9 @@ void rmw_uxrce_fini_service_memory(rmw_service_t * service)
   }
   if (service->data) {
     rmw_uxrce_service_t * custom_service = (rmw_uxrce_service_t *)service->data;
+    custom_service->rmw_handle = NULL;
 
     put_memory(&service_memory, &custom_service->mem);
-
-    memset(custom_service, 0, sizeof(rmw_uxrce_service_t));
     service->data = NULL;
   }
   rmw_free(service);
@@ -306,10 +305,9 @@ void rmw_uxrce_fini_client_memory(rmw_client_t * client)
   }
   if (client->data) {
     rmw_uxrce_client_t * custom_client = (rmw_uxrce_client_t *)client->data;
+    custom_client->rmw_handle = NULL;
 
     put_memory(&client_memory, &custom_client->mem);
-
-    memset(custom_client, 0, sizeof(rmw_uxrce_client_t));
     client->data = NULL;
   }
   rmw_free(client);

rmw_microxrcedds_c/src/types.h

@@ -197,6 +197,7 @@ typedef struct rmw_uxrce_publisher_t
 typedef struct rmw_uxrce_node_t
 {
   struct rmw_uxrce_mempool_item_t mem;
+  rmw_node_t * rmw_handle;
   struct  rmw_context_impl_t * context;
 
   uxrObjectId participant_id;

rmw_microxrcedds_c/test/CMakeLists.txt

@@ -47,8 +47,8 @@ macro(rmw_test TEST_NAME TEST_SOURCE)
 endmacro()
 
 rmw_test(test-node        test_node.cpp)
-# rmw_test(test-publisher   test_publisher.cpp)
-# rmw_test(test-subscriber  test_subscription.cpp)
-# rmw_test(test-pubsub      test_pubsub.cpp)
-# rmw_test(test-topic       test_topic.cpp)
-# rmw_test(test-rmw         test_rmw.cpp)
+rmw_test(test-publisher   test_publisher.cpp)
+rmw_test(test-subscriber  test_subscription.cpp)
+rmw_test(test-pubsub      test_pubsub.cpp)
+rmw_test(test-topic       test_topic.cpp)
+rmw_test(test-rmw         test_rmw.cpp)

rmw_microxrcedds_c/test/rmw_base_test.hpp

@@ -31,13 +31,13 @@ class RMWBaseTest : public ::testing::Test
 
   void SetUp() override
   {
-    EXPECT_EQ(rmw_init_options_init(&test_options, rcutils_get_default_allocator()), RMW_RET_OK);
-    EXPECT_EQ(rmw_init(&test_options, &test_context), RMW_RET_OK);
+    ASSERT_EQ(rmw_init_options_init(&test_options, rcutils_get_default_allocator()), RMW_RET_OK);
+    ASSERT_EQ(rmw_init(&test_options, &test_context), RMW_RET_OK);
   }
 
   void TearDown() override
   {
-    EXPECT_EQ(rmw_shutdown(&test_context), RMW_RET_OK);
+    ASSERT_EQ(rmw_shutdown(&test_context), RMW_RET_OK);
   }
 
   rmw_context_t test_context = rmw_get_zero_initialized_context();

rmw_microxrcedds_c/test/test_node.cpp

@@ -35,20 +35,20 @@ class TestNode : public RMWBaseTest
 TEST_F(TestNode, construction_and_destruction) {
   // Success creation
   rmw_node_t * node = rmw_create_node(&test_context, "my_node", "/ns", 0, false);
-  EXPECT_NE(node, nullptr);
+  ASSERT_NE(node, nullptr);
   rmw_ret_t ret = rmw_destroy_node(node);
-  EXPECT_EQ(ret, RMW_RET_OK);
-  EXPECT_EQ(CheckErrorState(), false);
+  ASSERT_EQ(ret, RMW_RET_OK);
+  ASSERT_EQ(CheckErrorState(), false);
 
   // Unsuccess creation
   node = rmw_create_node(&test_context, "", "/ns", 0, false);
-  EXPECT_EQ(node, nullptr);
-  EXPECT_EQ(CheckErrorState(), true);
+  ASSERT_EQ(node, nullptr);
+  ASSERT_EQ(CheckErrorState(), true);
   rcutils_reset_error();
 
   // Unsuccess creation
   node = rmw_create_node(&test_context, "my_node", "", 0, false);
-  EXPECT_EQ(node, nullptr);
+  ASSERT_EQ(node, nullptr);
   rcutils_reset_error();
 }
 
@@ -63,30 +63,45 @@ TEST_F(TestNode, memory_poll) {
   // Get all available nodes
   for (size_t i = 0; i < RMW_UXRCE_MAX_NODES; i++) {
     node = rmw_create_node(&test_context, "my_node", "/ns", 0, false);
-    EXPECT_NE(node, nullptr);
+    ASSERT_NE(node, nullptr);
     nodes.push_back(node);
   }
 
   // Try to get one
   node = rmw_create_node(&test_context, "my_node", "/ns", 0, false);
-  EXPECT_EQ(node, nullptr);
-  EXPECT_EQ(CheckErrorState(), true);
+  ASSERT_EQ(node, nullptr);
+  ASSERT_EQ(CheckErrorState(), true);
   rcutils_reset_error();
 
   // Relese one
   ret = rmw_destroy_node(nodes.back());
-  EXPECT_EQ(ret, RMW_RET_OK);
+  ASSERT_EQ(ret, RMW_RET_OK);
   nodes.pop_back();
 
   // Get one
   node = rmw_create_node(&test_context, "my_node", "/ns", 0, false);
-  EXPECT_NE(node, nullptr);
+  ASSERT_NE(node, nullptr);
   nodes.push_back(node);
 
   // Release all
   for (size_t i = 0; i < nodes.size(); i++) {
     ret = rmw_destroy_node(nodes.at(i));
-    EXPECT_EQ(ret, RMW_RET_OK);
+    ASSERT_EQ(ret, RMW_RET_OK);
   }
   nodes.clear();
+
+  // Get all available nodes
+  for (size_t i = 0; i < RMW_UXRCE_MAX_NODES; i++) {
+    node = rmw_create_node(&test_context, "my_node", "/ns", 0, false);
+    ASSERT_NE(node, nullptr);
+    nodes.push_back(node);
+  }
+
+  // Release all
+  for (size_t i = 0; i < nodes.size(); i++) {
+    ret = rmw_destroy_node(nodes.at(i));
+    ASSERT_EQ(ret, RMW_RET_OK);
+  }
+  nodes.clear();
+
 }