Fix rmw_wait lock (#200) (#201)

* Fix rmw_wait lock

* Fix

* Lock memory usage in ping

Lock fixes

Fix

fix

* Do not wait if no contexts

* Fix

* Uncrustify

* Refactor cleaning

* Unlock timing API

(cherry picked from commit 50cdbf8)

Co-authored-by: Pablo Garrido <pablogs9@gmail.com>

Author: mergify[bot]

rmw_microxrcedds_c/src/rmw_microros/ping.c

@@ -33,6 +33,8 @@ rmw_ret_t rmw_uros_ping_agent(
 {
   bool success = false;
 
+  UXR_LOCK(&session_memory.mutex);
+
   if (NULL == session_memory.allocateditems) {
 #ifdef RMW_UXRCE_TRANSPORT_SERIAL
     uxrSerialTransport transport;
@@ -52,6 +54,7 @@ rmw_ret_t rmw_uros_ping_agent(
     rmw_ret_t ret = rmw_uxrce_transport_init(NULL, NULL, (void *)&transport);
 
     if (RMW_RET_OK != ret) {
+      UXR_UNLOCK(&session_memory.mutex);
       return ret;
     }
 
@@ -67,6 +70,8 @@ rmw_ret_t rmw_uros_ping_agent(
     } while (NULL != item && !success);
   }
 
+  UXR_UNLOCK(&session_memory.mutex);
+
   return success ? RMW_RET_OK : RMW_RET_ERROR;
 }
 

rmw_microxrcedds_c/src/rmw_microros/time_sync.c

@@ -30,7 +30,8 @@ bool rmw_uros_epoch_synchronized()
   rmw_uxrce_mempool_item_t * item = session_memory.allocateditems;
   rmw_context_impl_t * context = (rmw_context_impl_t *)item->data;
 
-  return context->session.synchronized;
+  bool ret = context->session.synchronized;
+  return ret;
 }
 
 int64_t rmw_uros_epoch_millis()
@@ -44,7 +45,8 @@ int64_t rmw_uros_epoch_millis()
   rmw_uxrce_mempool_item_t * item = session_memory.allocateditems;
   rmw_context_impl_t * context = (rmw_context_impl_t *)item->data;
 
-  return uxr_epoch_millis(&context->session);
+  int64_t ret = uxr_epoch_millis(&context->session);
+  return ret;
 }
 
 int64_t rmw_uros_epoch_nanos()
@@ -58,7 +60,8 @@ int64_t rmw_uros_epoch_nanos()
   rmw_uxrce_mempool_item_t * item = session_memory.allocateditems;
   rmw_context_impl_t * context = (rmw_context_impl_t *)item->data;
 
-  return uxr_epoch_nanos(&context->session);
+  int64_t ret = uxr_epoch_nanos(&context->session);
+  return ret;
 }
 
 rmw_ret_t rmw_uros_sync_session(
@@ -77,8 +80,7 @@ rmw_ret_t rmw_uros_sync_session(
 
   if (!uxr_sync_session(&context->session, timeout_ms)) {
     RMW_SET_ERROR_MSG("Time synchronization failed.");
-    return RMW_RET_ERROR;
+    ret = RMW_RET_ERROR;
   }
-
   return ret;
 }

rmw_microxrcedds_c/src/rmw_wait.c

@@ -51,13 +51,12 @@ rmw_wait(
     timeout.i32 = (timeout.i64 > INT32_MAX) ? INT32_MAX : timeout.i64;
   }
 
-  // Clean expired buffers
   rmw_uxrce_clean_expired_static_input_buffer();
 
-  rmw_uxrce_mempool_item_t * item = NULL;
+  UXR_LOCK(&session_memory.mutex);
 
   // Clear run flag for all sessions
-  item = session_memory.allocateditems;
+  rmw_uxrce_mempool_item_t * item = session_memory.allocateditems;
   while (item != NULL) {
     rmw_context_impl_t * custom_context = (rmw_context_impl_t *)item->data;
     custom_context->need_to_be_ran = false;
@@ -90,7 +89,11 @@ rmw_wait(
     item = item->next;
   }
 
-  rmw_uxrce_clean_expired_static_input_buffer();
+  // There is no context that contais any of the wait set entities. Nothing to wait here.
+  if (available_contexts == 0) {
+    UXR_UNLOCK(&session_memory.mutex);
+    return RMW_RET_OK;
+  }
 
   int32_t per_session_timeout =
     (timeout.i32 == UXR_TIMEOUT_INF) ? UXR_TIMEOUT_INF :
@@ -105,6 +108,8 @@ rmw_wait(
     item = item->next;
   }
 
+  UXR_UNLOCK(&session_memory.mutex);
+
   bool buffered_status = false;
 
   // Check services

rmw_microxrcedds_c/src/types.c

@@ -224,9 +224,10 @@ size_t rmw_uxrce_count_static_input_buffer_for_entity(
   void * entity)
 {
   size_t count = 0;
-  rmw_uxrce_mempool_item_t * item = static_buffer_memory.allocateditems;
 
   UXR_LOCK(&static_buffer_memory.mutex);
+  rmw_uxrce_mempool_item_t * item = static_buffer_memory.allocateditems;
+
   while (item != NULL) {
     rmw_uxrce_static_input_buffer_t * data = (rmw_uxrce_static_input_buffer_t *)item->data;
     if (data->owner == entity) {