Update memory inits and fixes (#61)

* Update memory inits and fixes

* Updated timeout management

* rmw_wait logic

Author: pablogs9

rmw_microxrcedds_c/src/rmw_init.c

@@ -100,7 +100,10 @@ rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst)
     RMW_SET_ERROR_MSG("expected zero-initialized dst");
     return RMW_RET_INVALID_ARGUMENT;
   }
-  *dst = *src;
+  memcpy(dst, src, sizeof(rmw_init_options_t));
+  dst->impl = rmw_allocate(sizeof(rmw_init_options_impl_t));
+  memcpy(dst->impl, src->impl, sizeof(rmw_init_options_impl_t));
+
   return RMW_RET_OK;
 }
 

rmw_microxrcedds_c/src/rmw_wait.c

@@ -99,7 +99,8 @@ rmw_wait(
     session = &custom_subscription->owner_node->context->session;
   }
 
-  uxr_run_session_until_timeout(session, timeout);
+  bool run_session_status = uxr_run_session_until_timeout(session, timeout);
+  bool buffered_status = false;
 
   // Check services
   if (services) {
@@ -108,6 +109,8 @@ rmw_wait(
 
       if (!custom_service->micro_buffer_in_use){
         services->services[i] = NULL;
+      }else{
+        buffered_status = true;
       }
     }
   }
@@ -119,6 +122,8 @@ rmw_wait(
 
       if (!custom_client->micro_buffer_in_use){
         clients->clients[i] = NULL;
+      }else{
+        buffered_status = true;
       }
     }
   }
@@ -130,13 +135,14 @@ rmw_wait(
 
       if (!custom_subscription->micro_buffer_in_use){
         subscriptions->subscribers[i] = NULL;
+      }else{
+        buffered_status = true;
       }
     }
   }
 
   EPROS_PRINT_TRACE()
-
-  // TODO (Pablo): When it need to return a timeout?
-  ret = RMW_RET_OK;
-  return ret;
+    
+  return  (buffered_status) ? RMW_RET_OK : 
+          ((run_session_status) ? RMW_RET_ERROR : RMW_RET_TIMEOUT);
 }

rmw_microxrcedds_c/src/types.c

@@ -32,27 +32,34 @@
 
 struct rmw_uxrce_mempool_t session_memory;
 rmw_context_impl_t custom_sessions[RMW_UXRCE_MAX_SESSIONS];
+static bool session_memory_init = false;
 
 struct rmw_uxrce_mempool_t node_memory;
 rmw_uxrce_node_t custom_nodes[RMW_UXRCE_MAX_NODES];
+static bool node_memory_init = false;
 
 struct rmw_uxrce_mempool_t publisher_memory;
 rmw_uxrce_publisher_t custom_publishers[RMW_UXRCE_MAX_PUBLISHERS + RMW_UXRCE_MAX_NODES];
+static bool publisher_memory_init = false;
 
 struct rmw_uxrce_mempool_t subscription_memory;
 rmw_uxrce_subscription_t custom_subscriptions[RMW_UXRCE_MAX_SUBSCRIPTIONS];
+static bool subscription_memory_init = false;
 
 struct rmw_uxrce_mempool_t service_memory;
 rmw_uxrce_service_t custom_services[RMW_UXRCE_MAX_SERVICES];
+static bool service_memory_init = false;
 
 struct rmw_uxrce_mempool_t client_memory;
 rmw_uxrce_client_t custom_clients[RMW_UXRCE_MAX_CLIENTS];
+static bool client_memory_init = false;
 
 // Memory init functions
 
 void rmw_uxrce_init_service_memory(struct rmw_uxrce_mempool_t * memory, rmw_uxrce_service_t * services, size_t size)
 {
-  if (size > 0) {
+  if (size > 0 && !service_memory_init) {
+    service_memory_init = true;
     link_prev(NULL, &services[0].mem, NULL);
     size > 1 ? link_next(&services[0].mem, &services[1].mem, &services[0]) : link_next(
       &services[0].mem, NULL, &services[0]);
@@ -66,7 +73,8 @@ void rmw_uxrce_init_service_memory(struct rmw_uxrce_mempool_t * memory, rmw_uxrc
 
 void rmw_uxrce_init_client_memory(struct rmw_uxrce_mempool_t * memory, rmw_uxrce_client_t * clients, size_t size)
 {
-  if (size > 0) {
+  if (size > 0 && !client_memory_init) {
+    client_memory_init = true;
     link_prev(NULL, &clients[0].mem, NULL);
     size > 1 ? link_next(&clients[0].mem, &clients[1].mem, &clients[0]) : link_next(
       &clients[0].mem, NULL, &clients[0]);
@@ -80,7 +88,8 @@ void rmw_uxrce_init_client_memory(struct rmw_uxrce_mempool_t * memory, rmw_uxrce
 
 void rmw_uxrce_init_publisher_memory(struct rmw_uxrce_mempool_t * memory, rmw_uxrce_publisher_t * publishers, size_t size)
 {
-  if (size > 0) {
+  if (size > 0 && !publisher_memory_init) {
+    publisher_memory_init = true;
     link_prev(NULL, &publishers[0].mem, NULL);
     size > 1 ? link_next(&publishers[0].mem, &publishers[1].mem, &publishers[0]) : link_next(
       &publishers[0].mem, NULL, &publishers[0]);
@@ -94,7 +103,8 @@ void rmw_uxrce_init_publisher_memory(struct rmw_uxrce_mempool_t * memory, rmw_ux
 
 void rmw_uxrce_init_subscriber_memory(struct rmw_uxrce_mempool_t * memory, rmw_uxrce_subscription_t * subscribers, size_t size)
 {
-  if (size > 0) {
+  if (size > 0 && !subscription_memory_init) {
+    subscription_memory_init = true;
     link_prev(NULL, &subscribers[0].mem, NULL);
     size > 1 ? link_next(&subscribers[0].mem, &subscribers[1].mem, &subscribers[0]) : link_next(
       &subscribers[0].mem, NULL, &subscribers[0]);
@@ -108,7 +118,8 @@ void rmw_uxrce_init_subscriber_memory(struct rmw_uxrce_mempool_t * memory, rmw_u
 
 void rmw_uxrce_init_nodes_memory(struct rmw_uxrce_mempool_t * memory, rmw_uxrce_node_t * nodes, size_t size)
 {
-  if (size > 0) {
+  if (size > 0 && !node_memory_init) {
+    node_memory_init = true;
     link_prev(NULL, &nodes[0].mem, NULL);
     size > 1 ? link_next(&nodes[0].mem, &nodes[1].mem, &nodes[0]) : link_next(&nodes[0].mem, NULL,
       &nodes[0]);
@@ -122,7 +133,8 @@ void rmw_uxrce_init_nodes_memory(struct rmw_uxrce_mempool_t * memory, rmw_uxrce_
 
 void rmw_uxrce_init_sessions_memory(struct rmw_uxrce_mempool_t * memory, rmw_context_impl_t * sessions, size_t size)
 {
-  if (size > 0) {
+  if (size > 0 && !session_memory_init) {
+    session_memory_init = true;
     link_prev(NULL, &sessions[0].mem, NULL);
     size > 1 ? link_next(&sessions[0].mem, &sessions[1].mem, &sessions[0]) : link_next(&sessions[0].mem, NULL,
       &sessions[0]);