From d3a6240e00969b2be2861764654b274c12560a0d Mon Sep 17 00:00:00 2001
From: Pablo Garrido <pablogs9@gmail.com>
Date: Mon, 15 Nov 2021 16:25:37 +0100
Subject: [PATCH 1/4] XRCE Streams API

---
 .../include/rmw_microros/init_options.h       | 52 +++++++++++++
 rmw_microxrcedds_c/src/config.h.in            |  1 +
 rmw_microxrcedds_c/src/rmw_client.c           |  8 +-
 rmw_microxrcedds_c/src/rmw_init.c             | 43 +++++++----
 .../src/rmw_microros/init_options.c           | 76 +++++++++++++++++++
 .../src/rmw_microros_internal/types.h         | 28 +++++--
 rmw_microxrcedds_c/src/rmw_publish.c          | 10 ++-
 rmw_microxrcedds_c/src/rmw_publisher.c        |  9 ++-
 rmw_microxrcedds_c/src/rmw_service.c          |  8 +-
 rmw_microxrcedds_c/src/rmw_subscription.c     |  9 ++-
 rmw_microxrcedds_c/src/types.c                |  4 +
 rmw_microxrcedds_c/test/test_sizes.cpp        |  8 +-
 12 files changed, 218 insertions(+), 38 deletions(-)

diff --git a/rmw_microxrcedds_c/include/rmw_microros/init_options.h b/rmw_microxrcedds_c/include/rmw_microros/init_options.h
index f29d75a3..8d33be30 100644
--- a/rmw_microxrcedds_c/include/rmw_microros/init_options.h
+++ b/rmw_microxrcedds_c/include/rmw_microros/init_options.h
@@ -87,6 +87,58 @@ rmw_ret_t rmw_uros_options_set_client_key(
   uint32_t client_key,
   rmw_init_options_t * rmw_options);
 
+/**
+ * \brief Fills rmw implementation-specific options with the given parameters.
+ * In this case, the stream number of a publisher can be defined.
+ * This function may use static memory resources that should be freed.
+ *
+ * \param[in] stream Micro XRCE-DDS Client stream number.
+ * \param[in,out] rmw_options Updated options with rmw specifics.
+ * \return RMW_RET_OK If arguments were valid and set in rmw_init_options.
+ * \return RMW_RET_INVALID_ARGUMENT If rmw_init_options is not valid or unexpected arguments.
+ * \return RMW_RET_ERROR In other case.
+ */
+rmw_ret_t rmw_uros_set_publisher_out_stream(
+  size_t stream,
+  rmw_publisher_options_t * rmw_options);
+
+/**
+ * \brief Free rmw implementation-specific static resources allocated.
+ *
+ * \param[in,out] rmw_options RMW options with rmw specifics.
+ * \return RMW_RET_OK If arguments were valid and set in rmw_init_options.
+ * \return RMW_RET_INVALID_ARGUMENT If rmw_init_options is not valid or unexpected arguments.
+ * \return RMW_RET_ERROR In other case.
+ */
+rmw_ret_t rmw_uros_free_publisher_init_options(
+  rmw_publisher_options_t * rmw_options);
+
+/**
+ * \brief Fills rmw implementation-specific options with the given parameters.
+ * In this case, the stream number of a subscriber can be defined.
+ * This function may use static memory resources that should be freed.
+ *
+ * \param[in] stream Micro XRCE-DDS Client stream number.
+ * \param[in,out] rmw_options Updated options with rmw specifics.
+ * \return RMW_RET_OK If arguments were valid and set in rmw_init_options.
+ * \return RMW_RET_INVALID_ARGUMENT If rmw_init_options is not valid or unexpected arguments.
+ * \return RMW_RET_ERROR In other case.
+ */
+rmw_ret_t rmw_uros_set_subscriber_input_stream(
+  size_t stream,
+  rmw_subscription_options_t * rmw_options);
+
+/**
+ * \brief Free rmw implementation-specific static resources allocated.
+ *
+ * \param[in,out] rmw_options Options with rmw specifics.
+ * \return RMW_RET_OK If arguments were valid and set in rmw_init_options.
+ * \return RMW_RET_INVALID_ARGUMENT If rmw_init_options is not valid or unexpected arguments.
+ * \return RMW_RET_ERROR In other case.
+ */
+rmw_ret_t rmw_uros_free_subscriber_init_options(
+  rmw_subscription_options_t * rmw_options);
+
 /** @}*/
 
 #if defined(__cplusplus)
diff --git a/rmw_microxrcedds_c/src/config.h.in b/rmw_microxrcedds_c/src/config.h.in
index 29e74a88..a1e8b52e 100644
--- a/rmw_microxrcedds_c/src/config.h.in
+++ b/rmw_microxrcedds_c/src/config.h.in
@@ -59,6 +59,7 @@
 #define RMW_UXRCE_MAX_TOPICS @RMW_UXRCE_MAX_TOPICS@
 #define RMW_UXRCE_MAX_WAIT_SETS @RMW_UXRCE_MAX_WAIT_SETS@
 #define RMW_UXRCE_MAX_GUARD_CONDITION @RMW_UXRCE_MAX_GUARD_CONDITION@
+#define RMW_UXRCE_MAX_ENTITIES_INIT_OPTION RMW_UXRCE_MAX_PUBLISHERS + RMW_UXRCE_MAX_SUBSCRIPTIONS
 
 #if RMW_UXRCE_MAX_TOPICS == -1
 #define RMW_UXRCE_MAX_TOPICS_INTERNAL RMW_UXRCE_MAX_PUBLISHERS + RMW_UXRCE_MAX_SUBSCRIPTIONS
diff --git a/rmw_microxrcedds_c/src/rmw_client.c b/rmw_microxrcedds_c/src/rmw_client.c
index ccbd1adf..c0e33052 100644
--- a/rmw_microxrcedds_c/src/rmw_client.c
+++ b/rmw_microxrcedds_c/src/rmw_client.c
@@ -161,13 +161,13 @@ rmw_create_client(
 
     custom_client->stream_id =
       (qos_policies->reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) ?
-      custom_node->context->best_effort_output :
-      custom_node->context->reliable_output;
+      custom_node->context->best_effort_output[0] :
+      custom_node->context->reliable_output[0];
 
     uxrStreamId data_request_stream_id =
       (qos_policies->reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) ?
-      custom_node->context->best_effort_input :
-      custom_node->context->reliable_input;
+      custom_node->context->best_effort_input[0] :
+      custom_node->context->reliable_input[0];
 
     custom_client->client_data_request = uxr_buffer_request_data(
       &custom_node->context->session,
diff --git a/rmw_microxrcedds_c/src/rmw_init.c b/rmw_microxrcedds_c/src/rmw_init.c
index 3f635661..864bc064 100644
--- a/rmw_microxrcedds_c/src/rmw_init.c
+++ b/rmw_microxrcedds_c/src/rmw_init.c
@@ -269,6 +269,7 @@ rmw_init(
   rmw_uxrce_init_guard_condition_memory(
     &guard_condition_memory, custom_guard_condition,
     RMW_UXRCE_MAX_GUARD_CONDITION);
+  rmw_uxrce_init_entities_init_options_memory(&entities_init_options_memory, custom_entities_init_options, RMW_UXRCE_MAX_ENTITIES_INIT_OPTION);
 
   // Micro-XRCE-DDS Client transport initialization
   rmw_ret_t transport_init_ret = rmw_uxrce_transport_init(
@@ -286,20 +287,34 @@ rmw_init(
   uxr_set_request_callback(&context_impl->session, on_request, NULL);
   uxr_set_reply_callback(&context_impl->session, on_reply, NULL);
 
-  context_impl->reliable_input = uxr_create_input_reliable_stream(
-    &context_impl->session, context_impl->input_reliable_stream_buffer,
-    context_impl->transport.comm.mtu * RMW_UXRCE_STREAM_HISTORY_INPUT,
-    RMW_UXRCE_STREAM_HISTORY_INPUT);
-  context_impl->reliable_output =
-    uxr_create_output_reliable_stream(
-    &context_impl->session, context_impl->output_reliable_stream_buffer,
-    context_impl->transport.comm.mtu * RMW_UXRCE_STREAM_HISTORY_OUTPUT,
-    RMW_UXRCE_STREAM_HISTORY_OUTPUT);
-
-  context_impl->best_effort_input = uxr_create_input_best_effort_stream(&context_impl->session);
-  context_impl->best_effort_output = uxr_create_output_best_effort_stream(
-    &context_impl->session,
-    context_impl->output_best_effort_stream_buffer, context_impl->transport.comm.mtu);
+  for (size_t i = 0; i < UXR_CONFIG_MAX_INPUT_RELIABLE_STREAMS; i++)
+  {
+    context_impl->reliable_input[i] = uxr_create_input_reliable_stream(
+      &context_impl->session, context_impl->input_reliable_stream_buffer[i],
+      RMW_UXRCE_MAX_INPUT_BUFFER_SIZE,
+      RMW_UXRCE_STREAM_HISTORY_INPUT);
+  }
+
+  for (size_t i = 0; i < UXR_CONFIG_MAX_OUTPUT_RELIABLE_STREAMS; i++)
+  {
+    context_impl->reliable_output[i] =
+      uxr_create_output_reliable_stream(
+      &context_impl->session, context_impl->output_reliable_stream_buffer[i],
+      RMW_UXRCE_MAX_OUTPUT_BUFFER_SIZE,
+      RMW_UXRCE_STREAM_HISTORY_OUTPUT);
+  }
+
+  for (size_t i = 0; i < UXR_CONFIG_MAX_INPUT_BEST_EFFORT_STREAMS; i++)
+  {
+    context_impl->best_effort_input[i] = uxr_create_input_best_effort_stream(&context_impl->session);
+  }
+
+  for (size_t i = 0; i < UXR_CONFIG_MAX_OUTPUT_BEST_EFFORT_STREAMS; i++)
+  {
+    context_impl->best_effort_output[i] = uxr_create_output_best_effort_stream(
+      &context_impl->session,
+      context_impl->output_best_effort_stream_buffer[i], context_impl->transport.comm.mtu);
+  }
 
   if (!uxr_create_session(&context_impl->session)) {
     CLOSE_TRANSPORT(&context_impl->transport);
diff --git a/rmw_microxrcedds_c/src/rmw_microros/init_options.c b/rmw_microxrcedds_c/src/rmw_microros/init_options.c
index 1ff7eafd..30413a1e 100644
--- a/rmw_microxrcedds_c/src/rmw_microros/init_options.c
+++ b/rmw_microxrcedds_c/src/rmw_microros/init_options.c
@@ -128,3 +128,79 @@ rmw_ret_t rmw_uros_options_set_client_key(
 
   return RMW_RET_OK;
 }
+
+rmw_ret_t rmw_uros_set_publisher_out_stream(
+  size_t stream,
+  rmw_publisher_options_t * rmw_options)
+{
+  if (NULL == rmw_options) {
+    RMW_SET_ERROR_MSG("Uninitialised rmw_init_options.");
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+
+  if (NULL == rmw_options->rmw_specific_publisher_payload){
+    rmw_uxrce_mempool_item_t * memory_node = get_memory(&entities_init_options_memory);
+    if (NULL == memory_node) {
+      RMW_SET_ERROR_MSG("failed to allocate memory for publisher options");
+      return RMW_RET_ERROR;
+    }
+    rmw_options->rmw_specific_publisher_payload = memory_node->data;
+  }
+
+  rmw_uxrce_entities_init_options_t * uxrce_init_options = (rmw_uxrce_entities_init_options_t *) rmw_options->rmw_specific_publisher_payload;
+  uxrce_init_options->stream_index.publisher_output_stream = stream;
+
+  return RMW_RET_OK;
+}
+
+rmw_ret_t rmw_uros_free_publisher_init_options(
+  rmw_publisher_options_t * rmw_options)
+{
+  if (NULL == rmw_options || NULL == rmw_options->rmw_specific_subscription_payload) {
+    RMW_SET_ERROR_MSG("Uninitialised rmw_init_options.");
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+
+  put_memory(&entities_init_options_memory, &rmw_options->mem);
+  rmw_options->rmw_specific_subscription_payload = NULL;
+
+  return RMW_RET_OK;
+}
+
+rmw_ret_t rmw_uros_set_subscriber_input_stream(
+  size_t stream,
+  rmw_subscription_options_t * rmw_options)
+{
+  if (NULL == rmw_options) {
+    RMW_SET_ERROR_MSG("Uninitialised rmw_init_options.");
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+
+  if (NULL == rmw_options->rmw_specific_subscription_payload){
+    rmw_uxrce_mempool_item_t * memory_node = get_memory(&entities_init_options_memory);
+    if (NULL == memory_node) {
+      RMW_SET_ERROR_MSG("failed to allocate memory for subscriber options");
+      return RMW_RET_ERROR;
+    }
+    rmw_options->rmw_specific_subscription_payload = memory_node->data;
+  }
+
+  rmw_uxrce_entities_init_options_t * uxrce_init_options = (rmw_uxrce_entities_init_options_t *) rmw_options->rmw_specific_subscription_payload;
+  uxrce_init_options->stream_index.subscriber_input_stream = stream;
+
+  return RMW_RET_OK;
+}
+
+rmw_ret_t rmw_uros_free_subscriber_init_options(
+  rmw_subscription_options_t * rmw_options)
+{
+  if (NULL == rmw_options || NULL == rmw_options->rmw_specific_subscription_payload) {
+    RMW_SET_ERROR_MSG("Uninitialised rmw_init_options.");
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+
+  put_memory(&entities_init_options_memory, &rmw_options->mem);
+  rmw_options->rmw_specific_subscription_payload = NULL;
+
+  return RMW_RET_OK;
+}
diff --git a/rmw_microxrcedds_c/src/rmw_microros_internal/types.h b/rmw_microxrcedds_c/src/rmw_microros_internal/types.h
index 15d25f57..49031e78 100644
--- a/rmw_microxrcedds_c/src/rmw_microros_internal/types.h
+++ b/rmw_microxrcedds_c/src/rmw_microros_internal/types.h
@@ -76,19 +76,19 @@ typedef struct rmw_context_impl_t
 #endif  // if RMW_UXRCE_GRAPH
   rmw_guard_condition_t graph_guard_condition;
 
-  uxrStreamId reliable_input;
-  uxrStreamId reliable_output;
-  uxrStreamId best_effort_output;
-  uxrStreamId best_effort_input;
+  uxrStreamId reliable_input[UXR_CONFIG_MAX_INPUT_RELIABLE_STREAMS];
+  uxrStreamId reliable_output[UXR_CONFIG_MAX_OUTPUT_RELIABLE_STREAMS];
+  uxrStreamId best_effort_output[UXR_CONFIG_MAX_OUTPUT_BEST_EFFORT_STREAMS];
+  uxrStreamId best_effort_input[UXR_CONFIG_MAX_INPUT_BEST_EFFORT_STREAMS];
 
   uxrStreamId * creation_stream;
   uxrStreamId * destroy_stream;
   int creation_timeout;
   int destroy_timeout;
 
-  uint8_t input_reliable_stream_buffer[RMW_UXRCE_MAX_INPUT_BUFFER_SIZE];
-  uint8_t output_reliable_stream_buffer[RMW_UXRCE_MAX_OUTPUT_BUFFER_SIZE];
-  uint8_t output_best_effort_stream_buffer[RMW_UXRCE_MAX_TRANSPORT_MTU];
+  uint8_t input_reliable_stream_buffer[UXR_CONFIG_MAX_INPUT_RELIABLE_STREAMS][RMW_UXRCE_MAX_INPUT_BUFFER_SIZE];
+  uint8_t output_reliable_stream_buffer[UXR_CONFIG_MAX_OUTPUT_RELIABLE_STREAMS][RMW_UXRCE_MAX_OUTPUT_BUFFER_SIZE];
+  uint8_t output_best_effort_stream_buffer[UXR_CONFIG_MAX_OUTPUT_BEST_EFFORT_STREAMS][RMW_UXRCE_MAX_TRANSPORT_MTU];
 
   uint16_t id_participant;
   uint16_t id_topic;
@@ -113,6 +113,16 @@ struct rmw_init_options_impl_t
 
 typedef struct rmw_init_options_impl_t rmw_uxrce_init_options_impl_t;
 
+typedef struct rmw_uxrce_entities_init_options_t
+{
+  rmw_uxrce_mempool_item_t mem;
+
+  union {
+    size_t publisher_output_stream;
+    size_t subscriber_input_stream;
+  } stream_index;
+} rmw_uxrce_entities_init_options_t;
+
 // ROS2 entities definitions
 
 typedef struct rmw_uxrce_topic_t
@@ -294,6 +304,9 @@ extern rmw_uxrce_wait_set_t custom_wait_set[RMW_UXRCE_MAX_WAIT_SETS];
 extern rmw_uxrce_mempool_t guard_condition_memory;
 extern rmw_uxrce_guard_condition_t custom_guard_condition[RMW_UXRCE_MAX_GUARD_CONDITION];
 
+extern rmw_uxrce_mempool_t entities_init_options_memory;
+extern rmw_uxrce_entities_init_options_t custom_entities_init_options[RMW_UXRCE_MAX_ENTITIES_INIT_OPTION];
+
 // Memory init functions
 
 #define RMW_INIT_DEFINE_MEMORY(X) \
@@ -313,6 +326,7 @@ RMW_INIT_DEFINE_MEMORY(static_input_buffer)
 RMW_INIT_DEFINE_MEMORY(init_options_impl)
 RMW_INIT_DEFINE_MEMORY(wait_set)
 RMW_INIT_DEFINE_MEMORY(guard_condition)
+RMW_INIT_DEFINE_MEMORY(entities_init_options)
 
 // Memory management functions
 
diff --git a/rmw_microxrcedds_c/src/rmw_publish.c b/rmw_microxrcedds_c/src/rmw_publish.c
index 7e544e6d..276c0862 100644
--- a/rmw_microxrcedds_c/src/rmw_publish.c
+++ b/rmw_microxrcedds_c/src/rmw_publish.c
@@ -78,10 +78,14 @@ rmw_publish(
         custom_publisher->stream_id);
 
       if (UXR_BEST_EFFORT_STREAM == custom_publisher->stream_id.type) {
-        uxr_flash_output_streams(&custom_publisher->owner_node->context->session);
+        uxr_flash_one_output_stream(
+          &custom_publisher->owner_node->context->session,
+          custom_publisher->stream_id);
       } else {
-        written &= uxr_run_session_until_confirm_delivery(
-          &custom_publisher->owner_node->context->session, custom_publisher->session_timeout);
+        written &= uxr_run_session_until_confirm_delivery_one_stream(
+          &custom_publisher->owner_node->context->session,
+          custom_publisher->stream_id,
+          custom_publisher->session_timeout);
       }
     }
     if (!written) {
diff --git a/rmw_microxrcedds_c/src/rmw_publisher.c b/rmw_microxrcedds_c/src/rmw_publisher.c
index e4cb6c16..32b33dac 100644
--- a/rmw_microxrcedds_c/src/rmw_publisher.c
+++ b/rmw_microxrcedds_c/src/rmw_publisher.c
@@ -104,10 +104,15 @@ rmw_create_publisher(
     custom_publisher->session_timeout = RMW_UXRCE_PUBLISH_RELIABLE_TIMEOUT;
     custom_publisher->qos = *qos_policies;
 
+    rmw_uxrce_entities_init_options_t * uxrce_init_options = (rmw_uxrce_entities_init_options_t *) publisher_options->rmw_specific_publisher_payload;
+    size_t used_output_stream =
+      (NULL != uxrce_init_options) ?
+      uxrce_init_options->stream_index.publisher_output_stream : 0;
+
     custom_publisher->stream_id =
       (qos_policies->reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) ?
-      custom_node->context->best_effort_output :
-      custom_node->context->reliable_output;
+      custom_node->context->best_effort_output[used_output_stream] :
+      custom_node->context->reliable_output[used_output_stream];
 
     custom_publisher->cs_cb_size = NULL;
     custom_publisher->cs_cb_serialization = NULL;
diff --git a/rmw_microxrcedds_c/src/rmw_service.c b/rmw_microxrcedds_c/src/rmw_service.c
index ef7b4976..19136b7e 100644
--- a/rmw_microxrcedds_c/src/rmw_service.c
+++ b/rmw_microxrcedds_c/src/rmw_service.c
@@ -159,13 +159,13 @@ rmw_create_service(
 
     custom_service->stream_id =
       (qos_policies->reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) ?
-      custom_node->context->best_effort_output :
-      custom_node->context->reliable_output;
+      custom_node->context->best_effort_output[0] :
+      custom_node->context->reliable_output[0];
 
     uxrStreamId data_request_stream_id =
       (qos_policies->reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) ?
-      custom_node->context->best_effort_input :
-      custom_node->context->reliable_input;
+      custom_node->context->best_effort_input[0] :
+      custom_node->context->reliable_input[0];
 
     custom_service->service_data_resquest = uxr_buffer_request_data(
       &custom_node->context->session,
diff --git a/rmw_microxrcedds_c/src/rmw_subscription.c b/rmw_microxrcedds_c/src/rmw_subscription.c
index 394ba0ce..d15369b9 100644
--- a/rmw_microxrcedds_c/src/rmw_subscription.c
+++ b/rmw_microxrcedds_c/src/rmw_subscription.c
@@ -230,10 +230,15 @@ rmw_create_subscription(
     delivery_control.max_elapsed_time = UXR_MAX_ELAPSED_TIME_UNLIMITED;
     delivery_control.max_bytes_per_second = UXR_MAX_BYTES_PER_SECOND_UNLIMITED;
 
+    rmw_uxrce_entities_init_options_t * uxrce_init_options = (rmw_uxrce_entities_init_options_t *) subscription_options->rmw_specific_subscription_payload;
+    size_t used_input_stream =
+      (NULL != uxrce_init_options) ?
+      uxrce_init_options->stream_index.subscriber_input_stream : 0;
+
     uxrStreamId data_request_stream_id =
       (qos_policies->reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) ?
-      custom_node->context->best_effort_input :
-      custom_node->context->reliable_input;
+      custom_node->context->best_effort_input[used_input_stream] :
+      custom_node->context->reliable_input[used_input_stream];
 
     uxr_buffer_request_data(
       &custom_node->context->session,
diff --git a/rmw_microxrcedds_c/src/types.c b/rmw_microxrcedds_c/src/types.c
index 30353a67..3cf7fd6d 100644
--- a/rmw_microxrcedds_c/src/types.c
+++ b/rmw_microxrcedds_c/src/types.c
@@ -67,6 +67,9 @@ rmw_uxrce_wait_set_t custom_wait_set[RMW_UXRCE_MAX_WAIT_SETS];
 rmw_uxrce_mempool_t guard_condition_memory;
 rmw_uxrce_guard_condition_t custom_guard_condition[RMW_UXRCE_MAX_GUARD_CONDITION];
 
+rmw_uxrce_mempool_t entities_init_options_memory;
+rmw_uxrce_entities_init_options_t custom_entities_init_options[RMW_UXRCE_MAX_ENTITIES_INIT_OPTION];
+
 // Memory init functions
 
 #define RMW_INIT_MEMORY(X) \
@@ -103,6 +106,7 @@ RMW_INIT_MEMORY(static_input_buffer)
 RMW_INIT_MEMORY(init_options_impl)
 RMW_INIT_MEMORY(wait_set)
 RMW_INIT_MEMORY(guard_condition)
+RMW_INIT_MEMORY(entities_init_options)
 
 // Memory management functions
 
diff --git a/rmw_microxrcedds_c/test/test_sizes.cpp b/rmw_microxrcedds_c/test/test_sizes.cpp
index e076f79b..db624b96 100644
--- a/rmw_microxrcedds_c/test/test_sizes.cpp
+++ b/rmw_microxrcedds_c/test/test_sizes.cpp
@@ -39,6 +39,7 @@ TEST_F(RMWBaseTest, estimate_default_static_memory)
   uint64_t init_options_impl_size = sizeof(rmw_uxrce_init_options_impl_t);
   uint64_t wait_sets_size = sizeof(rmw_uxrce_wait_set_t);
   uint64_t guard_conditions_size = sizeof(rmw_uxrce_guard_condition_t);
+  uint64_t entities_init_option_size = sizeof(rmw_uxrce_entities_init_options_t);
 
   fprintf(stderr, "# Static memory analysis \n");
   fprintf(stderr, "_**Default configuration**_\n");
@@ -72,7 +73,9 @@ TEST_F(RMWBaseTest, estimate_default_static_memory)
   fprintf(
     stderr, "| Guard Condition | %d | %ld B | \n", RMW_UXRCE_MAX_GUARD_CONDITION,
     guard_conditions_size);
-
+  fprintf(
+    stderr, "| Ent. Init. Opt | %d | %ld B | \n", RMW_UXRCE_MAX_ENTITIES_INIT_OPTION,
+    entities_init_option_size);
 
   uint64_t total = RMW_UXRCE_MAX_SESSIONS * context_size +
     RMW_UXRCE_MAX_TOPICS_INTERNAL * topic_size +
@@ -84,7 +87,8 @@ TEST_F(RMWBaseTest, estimate_default_static_memory)
     RMW_UXRCE_MAX_HISTORY * static_input_buffer_size +
     RMW_UXRCE_MAX_OPTIONS * init_options_impl_size +
     RMW_UXRCE_MAX_WAIT_SETS * wait_sets_size +
-    RMW_UXRCE_MAX_GUARD_CONDITION * guard_conditions_size;
+    RMW_UXRCE_MAX_GUARD_CONDITION * guard_conditions_size +
+    RMW_UXRCE_MAX_ENTITIES_INIT_OPTION * entities_init_option_size;
 
   fprintf(stderr, "\n");
   fprintf(stderr, "**TOTAL: %ld B**\n", total);

From ffa42fbfb9958fe9b883849f3ef0ef09e579101f Mon Sep 17 00:00:00 2001
From: Pablo Garrido <pablogs9@gmail.com>
Date: Mon, 15 Nov 2021 16:28:58 +0100
Subject: [PATCH 2/4] Uncrustify

---
 rmw_microxrcedds_c/src/rmw_init.c             | 20 +++++++++----------
 .../src/rmw_microros/init_options.c           | 10 ++++++----
 .../src/rmw_microros_internal/types.h         | 12 +++++++----
 rmw_microxrcedds_c/src/rmw_publisher.c        |  3 ++-
 rmw_microxrcedds_c/src/rmw_subscription.c     |  3 ++-
 5 files changed, 28 insertions(+), 20 deletions(-)

diff --git a/rmw_microxrcedds_c/src/rmw_init.c b/rmw_microxrcedds_c/src/rmw_init.c
index 864bc064..ed05a565 100644
--- a/rmw_microxrcedds_c/src/rmw_init.c
+++ b/rmw_microxrcedds_c/src/rmw_init.c
@@ -269,7 +269,10 @@ rmw_init(
   rmw_uxrce_init_guard_condition_memory(
     &guard_condition_memory, custom_guard_condition,
     RMW_UXRCE_MAX_GUARD_CONDITION);
-  rmw_uxrce_init_entities_init_options_memory(&entities_init_options_memory, custom_entities_init_options, RMW_UXRCE_MAX_ENTITIES_INIT_OPTION);
+  rmw_uxrce_init_entities_init_options_memory(
+    &entities_init_options_memory,
+    custom_entities_init_options,
+    RMW_UXRCE_MAX_ENTITIES_INIT_OPTION);
 
   // Micro-XRCE-DDS Client transport initialization
   rmw_ret_t transport_init_ret = rmw_uxrce_transport_init(
@@ -287,16 +290,14 @@ rmw_init(
   uxr_set_request_callback(&context_impl->session, on_request, NULL);
   uxr_set_reply_callback(&context_impl->session, on_reply, NULL);
 
-  for (size_t i = 0; i < UXR_CONFIG_MAX_INPUT_RELIABLE_STREAMS; i++)
-  {
+  for (size_t i = 0; i < UXR_CONFIG_MAX_INPUT_RELIABLE_STREAMS; i++) {
     context_impl->reliable_input[i] = uxr_create_input_reliable_stream(
       &context_impl->session, context_impl->input_reliable_stream_buffer[i],
       RMW_UXRCE_MAX_INPUT_BUFFER_SIZE,
       RMW_UXRCE_STREAM_HISTORY_INPUT);
   }
 
-  for (size_t i = 0; i < UXR_CONFIG_MAX_OUTPUT_RELIABLE_STREAMS; i++)
-  {
+  for (size_t i = 0; i < UXR_CONFIG_MAX_OUTPUT_RELIABLE_STREAMS; i++) {
     context_impl->reliable_output[i] =
       uxr_create_output_reliable_stream(
       &context_impl->session, context_impl->output_reliable_stream_buffer[i],
@@ -304,13 +305,12 @@ rmw_init(
       RMW_UXRCE_STREAM_HISTORY_OUTPUT);
   }
 
-  for (size_t i = 0; i < UXR_CONFIG_MAX_INPUT_BEST_EFFORT_STREAMS; i++)
-  {
-    context_impl->best_effort_input[i] = uxr_create_input_best_effort_stream(&context_impl->session);
+  for (size_t i = 0; i < UXR_CONFIG_MAX_INPUT_BEST_EFFORT_STREAMS; i++) {
+    context_impl->best_effort_input[i] =
+      uxr_create_input_best_effort_stream(&context_impl->session);
   }
 
-  for (size_t i = 0; i < UXR_CONFIG_MAX_OUTPUT_BEST_EFFORT_STREAMS; i++)
-  {
+  for (size_t i = 0; i < UXR_CONFIG_MAX_OUTPUT_BEST_EFFORT_STREAMS; i++) {
     context_impl->best_effort_output[i] = uxr_create_output_best_effort_stream(
       &context_impl->session,
       context_impl->output_best_effort_stream_buffer[i], context_impl->transport.comm.mtu);
diff --git a/rmw_microxrcedds_c/src/rmw_microros/init_options.c b/rmw_microxrcedds_c/src/rmw_microros/init_options.c
index 30413a1e..1bdc42e8 100644
--- a/rmw_microxrcedds_c/src/rmw_microros/init_options.c
+++ b/rmw_microxrcedds_c/src/rmw_microros/init_options.c
@@ -138,7 +138,7 @@ rmw_ret_t rmw_uros_set_publisher_out_stream(
     return RMW_RET_INVALID_ARGUMENT;
   }
 
-  if (NULL == rmw_options->rmw_specific_publisher_payload){
+  if (NULL == rmw_options->rmw_specific_publisher_payload) {
     rmw_uxrce_mempool_item_t * memory_node = get_memory(&entities_init_options_memory);
     if (NULL == memory_node) {
       RMW_SET_ERROR_MSG("failed to allocate memory for publisher options");
@@ -147,7 +147,8 @@ rmw_ret_t rmw_uros_set_publisher_out_stream(
     rmw_options->rmw_specific_publisher_payload = memory_node->data;
   }
 
-  rmw_uxrce_entities_init_options_t * uxrce_init_options = (rmw_uxrce_entities_init_options_t *) rmw_options->rmw_specific_publisher_payload;
+  rmw_uxrce_entities_init_options_t * uxrce_init_options =
+    (rmw_uxrce_entities_init_options_t *) rmw_options->rmw_specific_publisher_payload;
   uxrce_init_options->stream_index.publisher_output_stream = stream;
 
   return RMW_RET_OK;
@@ -176,7 +177,7 @@ rmw_ret_t rmw_uros_set_subscriber_input_stream(
     return RMW_RET_INVALID_ARGUMENT;
   }
 
-  if (NULL == rmw_options->rmw_specific_subscription_payload){
+  if (NULL == rmw_options->rmw_specific_subscription_payload) {
     rmw_uxrce_mempool_item_t * memory_node = get_memory(&entities_init_options_memory);
     if (NULL == memory_node) {
       RMW_SET_ERROR_MSG("failed to allocate memory for subscriber options");
@@ -185,7 +186,8 @@ rmw_ret_t rmw_uros_set_subscriber_input_stream(
     rmw_options->rmw_specific_subscription_payload = memory_node->data;
   }
 
-  rmw_uxrce_entities_init_options_t * uxrce_init_options = (rmw_uxrce_entities_init_options_t *) rmw_options->rmw_specific_subscription_payload;
+  rmw_uxrce_entities_init_options_t * uxrce_init_options =
+    (rmw_uxrce_entities_init_options_t *) rmw_options->rmw_specific_subscription_payload;
   uxrce_init_options->stream_index.subscriber_input_stream = stream;
 
   return RMW_RET_OK;
diff --git a/rmw_microxrcedds_c/src/rmw_microros_internal/types.h b/rmw_microxrcedds_c/src/rmw_microros_internal/types.h
index 49031e78..9da1bef0 100644
--- a/rmw_microxrcedds_c/src/rmw_microros_internal/types.h
+++ b/rmw_microxrcedds_c/src/rmw_microros_internal/types.h
@@ -86,9 +86,12 @@ typedef struct rmw_context_impl_t
   int creation_timeout;
   int destroy_timeout;
 
-  uint8_t input_reliable_stream_buffer[UXR_CONFIG_MAX_INPUT_RELIABLE_STREAMS][RMW_UXRCE_MAX_INPUT_BUFFER_SIZE];
-  uint8_t output_reliable_stream_buffer[UXR_CONFIG_MAX_OUTPUT_RELIABLE_STREAMS][RMW_UXRCE_MAX_OUTPUT_BUFFER_SIZE];
-  uint8_t output_best_effort_stream_buffer[UXR_CONFIG_MAX_OUTPUT_BEST_EFFORT_STREAMS][RMW_UXRCE_MAX_TRANSPORT_MTU];
+  uint8_t input_reliable_stream_buffer[UXR_CONFIG_MAX_INPUT_RELIABLE_STREAMS][
+    RMW_UXRCE_MAX_INPUT_BUFFER_SIZE];
+  uint8_t output_reliable_stream_buffer[UXR_CONFIG_MAX_OUTPUT_RELIABLE_STREAMS][
+    RMW_UXRCE_MAX_OUTPUT_BUFFER_SIZE];
+  uint8_t output_best_effort_stream_buffer[UXR_CONFIG_MAX_OUTPUT_BEST_EFFORT_STREAMS][
+    RMW_UXRCE_MAX_TRANSPORT_MTU];
 
   uint16_t id_participant;
   uint16_t id_topic;
@@ -305,7 +308,8 @@ extern rmw_uxrce_mempool_t guard_condition_memory;
 extern rmw_uxrce_guard_condition_t custom_guard_condition[RMW_UXRCE_MAX_GUARD_CONDITION];
 
 extern rmw_uxrce_mempool_t entities_init_options_memory;
-extern rmw_uxrce_entities_init_options_t custom_entities_init_options[RMW_UXRCE_MAX_ENTITIES_INIT_OPTION];
+extern rmw_uxrce_entities_init_options_t custom_entities_init_options[
+  RMW_UXRCE_MAX_ENTITIES_INIT_OPTION];
 
 // Memory init functions
 
diff --git a/rmw_microxrcedds_c/src/rmw_publisher.c b/rmw_microxrcedds_c/src/rmw_publisher.c
index 32b33dac..711c0846 100644
--- a/rmw_microxrcedds_c/src/rmw_publisher.c
+++ b/rmw_microxrcedds_c/src/rmw_publisher.c
@@ -104,7 +104,8 @@ rmw_create_publisher(
     custom_publisher->session_timeout = RMW_UXRCE_PUBLISH_RELIABLE_TIMEOUT;
     custom_publisher->qos = *qos_policies;
 
-    rmw_uxrce_entities_init_options_t * uxrce_init_options = (rmw_uxrce_entities_init_options_t *) publisher_options->rmw_specific_publisher_payload;
+    rmw_uxrce_entities_init_options_t * uxrce_init_options =
+      (rmw_uxrce_entities_init_options_t *) publisher_options->rmw_specific_publisher_payload;
     size_t used_output_stream =
       (NULL != uxrce_init_options) ?
       uxrce_init_options->stream_index.publisher_output_stream : 0;
diff --git a/rmw_microxrcedds_c/src/rmw_subscription.c b/rmw_microxrcedds_c/src/rmw_subscription.c
index d15369b9..5d83c245 100644
--- a/rmw_microxrcedds_c/src/rmw_subscription.c
+++ b/rmw_microxrcedds_c/src/rmw_subscription.c
@@ -230,7 +230,8 @@ rmw_create_subscription(
     delivery_control.max_elapsed_time = UXR_MAX_ELAPSED_TIME_UNLIMITED;
     delivery_control.max_bytes_per_second = UXR_MAX_BYTES_PER_SECOND_UNLIMITED;
 
-    rmw_uxrce_entities_init_options_t * uxrce_init_options = (rmw_uxrce_entities_init_options_t *) subscription_options->rmw_specific_subscription_payload;
+    rmw_uxrce_entities_init_options_t * uxrce_init_options =
+      (rmw_uxrce_entities_init_options_t *) subscription_options->rmw_specific_subscription_payload;
     size_t used_input_stream =
       (NULL != uxrce_init_options) ?
       uxrce_init_options->stream_index.subscriber_input_stream : 0;

From 7598953a877f5bca24e43ede361398e002b5f4aa Mon Sep 17 00:00:00 2001
From: Pablo Garrido <pablogs9@gmail.com>
Date: Mon, 15 Nov 2021 16:31:18 +0100
Subject: [PATCH 3/4] Update CI

---
 .github/workflows/ci.yml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
index 1ab4857f..c61e6390 100644
--- a/.github/workflows/ci.yml
+++ b/.github/workflows/ci.yml
@@ -21,8 +21,8 @@ jobs:
 
         - name: Download dependencies
           run: |
-            git clone -b foxy https://github.com/eProsima/Micro-CDR src/Micro-CDR
-            git clone -b foxy https://github.com/eProsima/Micro-XRCE-DDS-Client src/Micro-XRCE-DDS-Client
+            git clone -b ros2 https://github.com/eProsima/Micro-CDR src/Micro-CDR
+            git clone -b feature/run_session_until_one_stream https://github.com/eProsima/Micro-XRCE-DDS-Client src/Micro-XRCE-DDS-Client
             git clone -b galactic https://github.com/micro-ROS/rosidl_typesupport_microxrcedds src/rosidl_typesupport_microxrcedds
             touch src/rosidl_typesupport_microxrcedds/test/COLCON_IGNORE
             # Install coverage tools

From db6e8e0e76199e3bce1a0f394b3dab6be05109d8 Mon Sep 17 00:00:00 2001
From: Pablo Garrido <pablogs9@gmail.com>
Date: Tue, 16 Nov 2021 07:35:20 +0100
Subject: [PATCH 4/4] Fixes

---
 rmw_microxrcedds_c/src/rmw_init.c                  |  8 ++++----
 rmw_microxrcedds_c/src/rmw_microros/init_options.c | 14 ++++++++++----
 2 files changed, 14 insertions(+), 8 deletions(-)

diff --git a/rmw_microxrcedds_c/src/rmw_init.c b/rmw_microxrcedds_c/src/rmw_init.c
index ed05a565..b116adc8 100644
--- a/rmw_microxrcedds_c/src/rmw_init.c
+++ b/rmw_microxrcedds_c/src/rmw_init.c
@@ -233,12 +233,12 @@ rmw_init(
   context_impl->destroy_timeout = RMW_UXRCE_ENTITY_DESTROY_TIMEOUT;
 
   context_impl->creation_stream = (RMW_UXRCE_ENTITY_CREATION_TIMEOUT > 0) ?
-    &context_impl->reliable_output :
-    &context_impl->best_effort_output;
+    &context_impl->reliable_output[0] :
+    &context_impl->best_effort_output[0];
 
   context_impl->destroy_stream = (RMW_UXRCE_ENTITY_DESTROY_TIMEOUT > 0) ?
-    &context_impl->reliable_output :
-    &context_impl->best_effort_output;
+    &context_impl->reliable_output[0] :
+    &context_impl->best_effort_output[0];
 
   context_impl->id_participant = 0;
   context_impl->id_topic = 0;
diff --git a/rmw_microxrcedds_c/src/rmw_microros/init_options.c b/rmw_microxrcedds_c/src/rmw_microros/init_options.c
index 1bdc42e8..5e527791 100644
--- a/rmw_microxrcedds_c/src/rmw_microros/init_options.c
+++ b/rmw_microxrcedds_c/src/rmw_microros/init_options.c
@@ -157,13 +157,16 @@ rmw_ret_t rmw_uros_set_publisher_out_stream(
 rmw_ret_t rmw_uros_free_publisher_init_options(
   rmw_publisher_options_t * rmw_options)
 {
-  if (NULL == rmw_options || NULL == rmw_options->rmw_specific_subscription_payload) {
+  if (NULL == rmw_options || NULL == rmw_options->rmw_specific_publisher_payload) {
     RMW_SET_ERROR_MSG("Uninitialised rmw_init_options.");
     return RMW_RET_INVALID_ARGUMENT;
   }
 
-  put_memory(&entities_init_options_memory, &rmw_options->mem);
-  rmw_options->rmw_specific_subscription_payload = NULL;
+  rmw_uxrce_entities_init_options_t * uxrce_init_options =
+    (rmw_uxrce_entities_init_options_t *) rmw_options->rmw_specific_publisher_payload;
+
+  put_memory(&entities_init_options_memory, &uxrce_init_options->mem);
+  rmw_options->rmw_specific_publisher_payload = NULL;
 
   return RMW_RET_OK;
 }
@@ -201,7 +204,10 @@ rmw_ret_t rmw_uros_free_subscriber_init_options(
     return RMW_RET_INVALID_ARGUMENT;
   }
 
-  put_memory(&entities_init_options_memory, &rmw_options->mem);
+  rmw_uxrce_entities_init_options_t * uxrce_init_options =
+    (rmw_uxrce_entities_init_options_t *) rmw_options->rmw_specific_subscription_payload;
+
+  put_memory(&entities_init_options_memory, &uxrce_init_options->mem);
   rmw_options->rmw_specific_subscription_payload = NULL;
 
   return RMW_RET_OK;