Update wait until data (#75)

* Update wait until data

* Adjust pub sub test to only one call to rmw_wait

* Update CI

* Update approach

* Revert "Update CI"

This reverts commit 9f900dc.

Author: pablogs9

rmw_microxrcedds_c/src/rmw_wait.c

@@ -64,27 +64,23 @@ rmw_wait(
     timeout = (uint64_t)UXR_TIMEOUT_INF;
   }
 
-  // Look for the XRCE session
-  uxrSession * session = NULL;
-
-  for (size_t i = 0; i < services->service_count && session == NULL; i++) {
-    rmw_uxrce_service_t * custom_service = (rmw_uxrce_service_t *)services->services[i];
-    session = &custom_service->owner_node->context->session;
-  }
-
-  for (size_t i = 0; i < clients->client_count && session == NULL; i++) {
-    rmw_uxrce_client_t * custom_client = (rmw_uxrce_client_t *)clients->clients[i];
-    session = &custom_client->owner_node->context->session;
-  }
-
-  for (size_t i = 0; i < subscriptions->subscriber_count && session == NULL; ++i) {
-    rmw_uxrce_subscription_t * custom_subscription =
-      (rmw_uxrce_subscription_t *)subscriptions->subscribers[i];
-    session = &custom_subscription->owner_node->context->session;
+  // Run every XRCE session
+
+  uint8_t available_contexts = 0;
+  struct rmw_uxrce_mempool_item_t * item = NULL;
+  
+  item = session_memory.allocateditems;
+  while (item != NULL) {
+    item = item->next;
+    available_contexts++;
   }
 
-  if (session != NULL) {
-    uxr_run_session_until_timeout(session, timeout);
+  uint64_t per_session_timeout = (uint64_t)((float)timeout/(float)available_contexts);
+  item = session_memory.allocateditems;
+  while (item != NULL) {
+    rmw_context_impl_t * custom_context = (rmw_context_impl_t *)item->data;
+    uxr_run_session_until_data(&custom_context->session, per_session_timeout);
+    item = item->next;
   }
 
   bool buffered_status = false;

rmw_microxrcedds_c/test/test_pubsub.cpp

@@ -118,48 +118,44 @@ TEST_F(TestPubSub, publish_and_receive) {
 
   ASSERT_EQ(rmw_publish(pub, &ros_message, NULL), RMW_RET_OK);
 
-  void * aux_ptr;
-  do {
-    aux_ptr = sub->data;
-    rmw_subscriptions_t subscriptions;
-    subscriptions.subscribers = &aux_ptr;
-    subscriptions.subscriber_count = 1;
+  rmw_subscriptions_t subscriptions;
+  subscriptions.subscribers = &sub->data;
+  subscriptions.subscriber_count = 1;
 
-    rmw_guard_conditions_t guard_conditions;
-    guard_conditions.guard_condition_count = 0;
+  rmw_guard_conditions_t guard_conditions;
+  guard_conditions.guard_condition_count = 0;
 
-    rmw_services_t services;
-    services.service_count = 0;
+  rmw_services_t services;
+  services.service_count = 0;
 
-    rmw_clients_t clients;
-    clients.client_count = 0;
+  rmw_clients_t clients;
+  clients.client_count = 0;
 
-    rmw_events_t * events = NULL;
-    rmw_wait_set_t * wait_set = NULL;
-    rmw_time_t wait_timeout;
+  rmw_events_t * events = NULL;
+  rmw_wait_set_t * wait_set = NULL;
+  rmw_time_t wait_timeout;
 
+  wait_timeout.sec = 1;
 
-    wait_timeout.sec = 1;
+  rmw_ret_t ret = rmw_wait(
+    &subscriptions,
+    &guard_conditions,
+    &services,
+    &clients,
+    events,
+    wait_set,
+    &wait_timeout
+  );
 
-    // TODO(pablogs9): Ensure here that the wait works the first time with the Micro XRCE-DDS run session until data message update 
-    rmw_ret_t ret = rmw_wait(
-      &subscriptions,
-      &guard_conditions,
-      &services,
-      &clients,
-      events,
-      wait_set,
-      &wait_timeout
-    );
-  } while (aux_ptr == NULL);
+  ASSERT_NE((void *)subscriptions.subscribers[0], (void *)NULL);
 
   rosidl_runtime_c__String read_ros_message;
   char buff[100];
   read_ros_message.data = buff;
   read_ros_message.capacity = sizeof(buff);
   read_ros_message.size = 0;
 
-  bool taken;
+  bool taken = false;
   ASSERT_EQ(
     rmw_take_with_info(
       sub,
@@ -168,6 +164,7 @@ TEST_F(TestPubSub, publish_and_receive) {
       NULL,
       NULL
     ), RMW_RET_OK);
+  
   ASSERT_EQ(taken, true);
   ASSERT_EQ(strcmp(content, read_ros_message.data), 0);
   ASSERT_EQ(strcmp(ros_message.data, read_ros_message.data), 0);