RMW QoS incoming buffer handling (backport #165) (#177)

mergify[bot] · pablogs9 · Acuadros95 · web-flow · commit 642bd161f4d3 · 2021-09-28T19:58:21.000+02:00
* RMW QoS incoming buffer handling (#165) * Initial * Update * Uncrustify * Fixes * Tests pubsub * Add two subs test * Uncrustify * Initial reqres * Adding complete req res tests * Uncrustify * Use absolute time reference * Avoid future data * Speedup tests * Update * Update * Pub/sub in best effort * Fix * Revert "Pub/sub in best effort" This reverts commit 9f46d80. * Revert "Fix" This reverts commit cb7a130. * Major review of rmw_wait * Uncrustify * Adapt Cmake * client branch * Update * Update * Uncrustify * Update rmw_microxrcedds_c/CMakeLists.txt Co-authored-by: Antonio Cuadros <49162117+Acuadros95@users.noreply.github.com> * Update rmw_microxrcedds_c/src/config.h.in Co-authored-by: Antonio Cuadros <49162117+Acuadros95@users.noreply.github.com> * Fix on_topic callback * Fix timeout handling * Exit if deserialize fails * Update .github/workflows/ci.yml Co-authored-by: Pablo Garrido <pablogs9@gmail.com> Co-authored-by: Antonio Cuadros <49162117+Acuadros95@users.noreply.github.com> Co-authored-by: Antonio cuadros <acuadros1995@gmail.com> (cherry picked from commit b1531c7) # Conflicts: # rmw_microxrcedds_c/test/test_pubsub.cpp # rmw_microxrcedds_c/test/test_reqres.cpp * resolve conflicts * Fix * fix * Update timeout * Fix time * Fix * Fix tests * Fix time unit Co-authored-by: Pablo Garrido <pablogs9@gmail.com> Co-authored-by: Antonio cuadros <acuadros1995@gmail.com>
diff --git a/rmw_microxrcedds_c/CMakeLists.txt b/rmw_microxrcedds_c/CMakeLists.txt
@@ -116,10 +116,6 @@ if(${RMW_UXRCE_CREATION_MODE} STREQUAL "refs")
   set(RMW_UXRCE_USE_REFS ON)
 endif()
 
-# Create source files with the define
-configure_file(${PROJECT_SOURCE_DIR}/src/config.h.in
-  ${PROJECT_BINARY_DIR}/include/rmw_microxrcedds_c/config.h)
-
 # Set install directories
 if(WIN32)
     set(DOC_DIR "doc")
@@ -286,12 +282,19 @@ if(BUILD_TESTING)
   # Pedantic in CI
   set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Werror")
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror")
+  if(RMW_UXRCE_MAX_SESSIONS STREQUAL "1")
+    set(RMW_UXRCE_MAX_SESSIONS "2")
+  endif()
 
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
   add_subdirectory(test)
 endif()
 
+# Create source files with the define
+configure_file(${PROJECT_SOURCE_DIR}/src/config.h.in
+  ${PROJECT_BINARY_DIR}/include/rmw_microxrcedds_c/config.h)
+
 # Documentation
 if(BUILD_DOCUMENTATION)
     find_package(Doxygen)
diff --git a/rmw_microxrcedds_c/src/callbacks.c b/rmw_microxrcedds_c/src/callbacks.c
@@ -70,26 +70,31 @@ void on_topic(
     if ((custom_subscription->datareader_id.id == object_id.id) &&
       (custom_subscription->datareader_id.type == object_id.type))
     {
-      rmw_uxrce_mempool_item_t * memory_node = get_memory(&static_buffer_memory);
+      rmw_uxrce_mempool_item_t * memory_node = rmw_uxrce_get_static_input_buffer_for_entity(
+        custom_subscription, custom_subscription->qos);
       if (!memory_node) {
         RMW_SET_ERROR_MSG("Not available static buffer memory node");
         return;
       }
 
       rmw_uxrce_static_input_buffer_t * static_buffer =
         (rmw_uxrce_static_input_buffer_t *)memory_node->data;
-      static_buffer->owner = (void *) custom_subscription;
-      static_buffer->length = length;
 
       if (!ucdr_deserialize_array_uint8_t(
           ub,
           static_buffer->buffer,
           length))
       {
         put_memory(&static_buffer_memory, memory_node);
+        return;
       }
 
-      break;
+      static_buffer->owner = (void *) custom_subscription;
+      static_buffer->length = length;
+      static_buffer->timestamp = rmw_uros_epoch_nanos();
+      static_buffer->entity_type = RMW_UXRCE_ENTITY_TYPE_SUBSCRIPTION;
+
+      return;
     }
     subscription_item = subscription_item->next;
   }
@@ -114,27 +119,32 @@ void on_request(
     // Check if request is related to the service
     rmw_uxrce_service_t * custom_service = (rmw_uxrce_service_t *)service_item->data;
     if (custom_service->service_data_resquest == request_id) {
-      rmw_uxrce_mempool_item_t * memory_node = get_memory(&static_buffer_memory);
+      rmw_uxrce_mempool_item_t * memory_node = rmw_uxrce_get_static_input_buffer_for_entity(
+        custom_service, custom_service->qos);
       if (!memory_node) {
         RMW_SET_ERROR_MSG("Not available static buffer memory node");
         return;
       }
 
       rmw_uxrce_static_input_buffer_t * static_buffer =
         (rmw_uxrce_static_input_buffer_t *)memory_node->data;
-      static_buffer->owner = (void *) custom_service;
-      static_buffer->length = length;
-      static_buffer->related.sample_id = *sample_id;
 
       if (!ucdr_deserialize_array_uint8_t(
           ub,
           static_buffer->buffer,
           length))
       {
         put_memory(&static_buffer_memory, memory_node);
+        return;
       }
 
-      break;
+      static_buffer->owner = (void *) custom_service;
+      static_buffer->length = length;
+      static_buffer->related.sample_id = *sample_id;
+      static_buffer->timestamp = rmw_uros_epoch_nanos();
+      static_buffer->entity_type = RMW_UXRCE_ENTITY_TYPE_SERVICE;
+
+      return;
     }
     service_item = service_item->next;
   }
@@ -159,27 +169,32 @@ void on_reply(
     // Check if reply is related to the client
     rmw_uxrce_client_t * custom_client = (rmw_uxrce_client_t *)client_item->data;
     if (custom_client->client_data_request == request_id) {
-      rmw_uxrce_mempool_item_t * memory_node = get_memory(&static_buffer_memory);
+      rmw_uxrce_mempool_item_t * memory_node = rmw_uxrce_get_static_input_buffer_for_entity(
+        custom_client, custom_client->qos);
       if (!memory_node) {
         RMW_SET_ERROR_MSG("Not available static buffer memory node");
         return;
       }
 
       rmw_uxrce_static_input_buffer_t * static_buffer =
         (rmw_uxrce_static_input_buffer_t *)memory_node->data;
-      static_buffer->owner = (void *) custom_client;
-      static_buffer->length = length;
-      static_buffer->related.reply_id = reply_id;
 
       if (!ucdr_deserialize_array_uint8_t(
           ub,
           static_buffer->buffer,
           length))
       {
         put_memory(&static_buffer_memory, memory_node);
+        return;
       }
 
-      break;
+      static_buffer->owner = (void *) custom_client;
+      static_buffer->length = length;
+      static_buffer->related.reply_id = reply_id;
+      static_buffer->timestamp = rmw_uros_epoch_nanos();
+      static_buffer->entity_type = RMW_UXRCE_ENTITY_TYPE_CLIENT;
+
+      return;
     }
     client_item = client_item->next;
   }
diff --git a/rmw_microxrcedds_c/src/rmw_client.c b/rmw_microxrcedds_c/src/rmw_client.c
@@ -70,6 +70,7 @@ rmw_create_client(
     custom_client->rmw_handle = rmw_client;
     custom_client->owner_node = custom_node;
     custom_client->session_timeout = RMW_UXRCE_PUBLISH_RELIABLE_TIMEOUT;
+    custom_client->qos = *qos_policies;
 
     const rosidl_service_type_support_t * type_support_xrce = NULL;
 #ifdef ROSIDL_TYPESUPPORT_MICROXRCEDDS_C__IDENTIFIER_VALUE
diff --git a/rmw_microxrcedds_c/src/rmw_init.c b/rmw_microxrcedds_c/src/rmw_init.c
@@ -283,6 +283,14 @@ rmw_shutdown(
     *context = rmw_get_zero_initialized_context();
   }
 
+  rmw_uxrce_mempool_item_t * item = static_buffer_memory.allocateditems;
+
+  while (item != NULL) {
+    rmw_uxrce_mempool_item_t * aux_next = item->next;
+    put_memory(&static_buffer_memory, item);
+    item = aux_next;
+  }
+
   return ret;
 }
 
diff --git a/rmw_microxrcedds_c/src/rmw_publisher.c b/rmw_microxrcedds_c/src/rmw_publisher.c
@@ -102,8 +102,7 @@ rmw_create_publisher(
     custom_publisher->rmw_handle = rmw_publisher;
     custom_publisher->owner_node = custom_node;
     custom_publisher->session_timeout = RMW_UXRCE_PUBLISH_RELIABLE_TIMEOUT;
-
-    memcpy(&custom_publisher->qos, qos_policies, sizeof(rmw_qos_profile_t));
+    custom_publisher->qos = *qos_policies;
 
     custom_publisher->stream_id =
       (qos_policies->reliability == RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT) ?
@@ -224,6 +223,7 @@ rmw_create_publisher(
       custom_publisher->topic->topic_id,
       reliability,
       history,
+      qos_policies->depth,
       durability,
       UXR_REPLACE | UXR_REUSE);
   #endif /* ifdef RMW_UXRCE_USE_REFS */
diff --git a/rmw_microxrcedds_c/src/rmw_request.c b/rmw_microxrcedds_c/src/rmw_request.c
@@ -81,6 +81,8 @@ rmw_take_request(
 
   rmw_uxrce_service_t * custom_service = (rmw_uxrce_service_t *)service->data;
 
+  rmw_uxrce_clean_expired_static_input_buffer();
+
   // Find first related item in static buffer memory pool
   rmw_uxrce_mempool_item_t * static_buffer_item =
     rmw_uxrce_find_static_input_buffer_by_owner((void *) custom_service);
diff --git a/rmw_microxrcedds_c/src/rmw_response.c b/rmw_microxrcedds_c/src/rmw_response.c
@@ -95,6 +95,8 @@ rmw_take_response(
 
   rmw_uxrce_client_t * custom_client = (rmw_uxrce_client_t *)client->data;
 
+  rmw_uxrce_clean_expired_static_input_buffer();
+
   // Find first related item in static buffer memory pool
   rmw_uxrce_mempool_item_t * static_buffer_item =
     rmw_uxrce_find_static_input_buffer_by_owner((void *) custom_client);
diff --git a/rmw_microxrcedds_c/src/rmw_service.c b/rmw_microxrcedds_c/src/rmw_service.c
@@ -69,6 +69,7 @@ rmw_create_service(
 
     custom_service->owner_node = custom_node;
     custom_service->session_timeout = RMW_UXRCE_PUBLISH_RELIABLE_TIMEOUT;
+    custom_service->qos = *qos_policies;
 
     const rosidl_service_type_support_t * type_support_xrce = NULL;
 #ifdef ROSIDL_TYPESUPPORT_MICROXRCEDDS_C__IDENTIFIER_VALUE
diff --git a/rmw_microxrcedds_c/src/rmw_subscription.c b/rmw_microxrcedds_c/src/rmw_subscription.c
@@ -101,9 +101,8 @@ rmw_create_subscription(
 
     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;
-    memcpy(&custom_subscription->qos, qos_policies, sizeof(rmw_qos_profile_t));
+    custom_subscription->qos = *qos_policies;
 
     const rosidl_message_type_support_t * type_support_xrce = NULL;
 #ifdef ROSIDL_TYPESUPPORT_MICROXRCEDDS_C__IDENTIFIER_VALUE
@@ -210,6 +209,7 @@ rmw_create_subscription(
       custom_subscription->topic->topic_id,
       reliability,
       history,
+      qos_policies->depth,
       durability,
       UXR_REPLACE | UXR_REUSE);
 #endif /* ifdef RMW_UXRCE_USE_XML */
diff --git a/rmw_microxrcedds_c/src/rmw_take.c b/rmw_microxrcedds_c/src/rmw_take.c
@@ -49,6 +49,8 @@ rmw_take_with_info(
 
   rmw_uxrce_subscription_t * custom_subscription = (rmw_uxrce_subscription_t *)subscription->data;
 
+  rmw_uxrce_clean_expired_static_input_buffer();
+
   // Find first related item in static buffer memory pool
   rmw_uxrce_mempool_item_t * static_buffer_item = rmw_uxrce_find_static_input_buffer_by_owner(
     (void *) custom_subscription);
diff --git a/rmw_microxrcedds_c/src/rmw_wait.c b/rmw_microxrcedds_c/src/rmw_wait.c
diff --git a/rmw_microxrcedds_c/src/types.c b/rmw_microxrcedds_c/src/types.c
diff --git a/rmw_microxrcedds_c/src/types.h b/rmw_microxrcedds_c/src/types.h
diff --git a/rmw_microxrcedds_c/test/test_pubsub.cpp b/rmw_microxrcedds_c/test/test_pubsub.cpp
diff --git a/rmw_microxrcedds_c/test/test_reqres.cpp b/rmw_microxrcedds_c/test/test_reqres.cpp

Original file line number	Diff line number	Diff line change
`@@ -283,6 +283,14 @@ rmw_shutdown(`
`283`	`283`	`*context = rmw_get_zero_initialized_context();`
`284`	`284`	`}`
`285`	`285`
	`286`	`+ rmw_uxrce_mempool_item_t * item = static_buffer_memory.allocateditems;`
	`287`	`+`
	`288`	`+ while (item != NULL) {`
	`289`	`+ rmw_uxrce_mempool_item_t * aux_next = item->next;`
	`290`	`+ put_memory(&static_buffer_memory, item);`
	`291`	`+ item = aux_next;`
	`292`	`+ }`
	`293`	`+`
`286`	`294`	`return ret;`
`287`	`295`	`}`
`288`	`296`