Runtime error handling API (backport #212) (#219)

* Runtime error handling API (#212)

* Initial

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

Uncrustify

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

Update

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

Uncrus

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

* Update

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

* Refactor

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

* Update

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

* Update

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

* Remove include old error handler

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

* Update

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

* Client

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

* Update

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

* Uncrustify

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

* Fix

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

* Fix

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

* Fix

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

* Update rmw_microxrcedds_c/src/rmw_microros_internal/error_handling_internal.h

Co-authored-by: Antonio Cuadros <49162117+Acuadros95@users.noreply.github.com>
(cherry picked from commit 266248d)

# Conflicts:
#	rmw_microxrcedds_c/src/rmw_get_endpoint_network_flow.c
#	rmw_microxrcedds_c/src/rmw_qos_profile_check_compatible.c
#	rmw_microxrcedds_c/src/rmw_wait.c

* Fix conflicts

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

* Revert "Fix conflicts"

This reverts commit 1db99e0.

* Fix conflicts

Signed-off-by: Pablo Garrido <pablogs9@gmail.com>

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

Author: mergify[bot]

rmw_microxrcedds_c/CMakeLists.txt

@@ -35,6 +35,7 @@ find_package(rmw REQUIRED)
 # Build options
 option(BUILD_DOCUMENTATION "Use doxygen to create product documentation" OFF)
 option(RMW_UXRCE_GRAPH "Allows to perform graph-related operations to the user" OFF)
+option(RMW_UROS_ERROR_HANDLING "Provides error handling callback functionality to user-space" OFF)
 
 if(RMW_UXRCE_GRAPH)
   find_package(micro_ros_msgs REQUIRED)
@@ -146,6 +147,10 @@ endif()
 
 set(DOC_INSTALL_DIR ${DOC_DIR} CACHE PATH "Installation directory for documentation")
 
+if(BUILD_TESTING)
+  set(RMW_UROS_ERROR_HANDLING ON)
+endif()
+
 set(SRCS
   src/identifiers.c
   src/memory.c
@@ -190,6 +195,7 @@ set(SRCS
   $<$<OR:$<BOOL:${RMW_UXRCE_TRANSPORT_UDP}>,$<BOOL:${RMW_UXRCE_TRANSPORT_TCP}>>:src/rmw_microros/discovery.c>
   $<$<BOOL:${RMW_UXRCE_TRANSPORT_CUSTOM}>:src/rmw_microros/custom_transport.c>
   $<$<BOOL:${RMW_UXRCE_GRAPH}>:src/rmw_graph.c>
+  $<$<BOOL:${RMW_UROS_ERROR_HANDLING}>:src/rmw_microros/error_handling.c>
 )
 
 add_library(${PROJECT_NAME}

rmw_microxrcedds_c/include/rmw_microros/error_handling.h

@@ -0,0 +1,96 @@
+// Copyright 2021 Proyectos y Sistemas de Mantenimiento SL (eProsima).
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/**
+ * @file
+ */
+
+#ifndef RMW_MICROROS__ERROR_HANDLING_H_
+#define RMW_MICROROS__ERROR_HANDLING_H_
+
+#ifdef RMW_UROS_ERROR_HANDLING
+
+#include <ucdr/microcdr.h>
+#include <rosidl_typesupport_microxrcedds_c/message_type_support.h>
+#include <rosidl_typesupport_microxrcedds_c/service_type_support.h>
+
+#if defined(__cplusplus)
+extern "C"
+{
+#endif  // if defined(__cplusplus)
+
+typedef enum
+{
+  RMW_UROS_ERROR_ON_UNKNOWN = 0,
+  RMW_UROS_ERROR_ON_NODE,
+  RMW_UROS_ERROR_ON_SERVICE,
+  RMW_UROS_ERROR_ON_CLIENT,
+  RMW_UROS_ERROR_ON_SUBSCRIPTION,
+  RMW_UROS_ERROR_ON_PUBLISHER,
+  RMW_UROS_ERROR_ON_GRAPH,
+  RMW_UROS_ERROR_ON_GUARD_CONDITION,
+  RMW_UROS_ERROR_ON_TOPIC
+} rmw_uros_error_entity_type_t;
+
+typedef enum
+{
+  RMW_UROS_ERROR_ENTITY_CREATION = 0,
+  RMW_UROS_ERROR_ENTITY_DESTRUCTION,
+  RMW_UROS_ERROR_CHECK,
+  RMW_UROS_ERROR_NOT_IMPLEMENTED,
+  RMW_UROS_ERROR_MIDDLEWARE_ALLOCATION,
+} rmw_uros_error_source_t;
+
+typedef struct
+{
+  const char * node;
+  const char * node_namespace;
+  const char * topic_name;
+  const ucdrBuffer * ucdr;
+  const size_t size;
+  union {
+    const message_type_support_callbacks_t * message_callbacks;
+    const service_type_support_callbacks_t * service_callbacks;
+  } type_support;
+  const char * description;
+} rmw_uros_error_context_t;
+
+typedef void (* rmw_uros_error_handling)(
+  const rmw_uros_error_entity_type_t entity,
+  const rmw_uros_error_source_t source,
+  const rmw_uros_error_context_t context,
+  const char * file,
+  const int line);
+
+/** \addtogroup rmw micro-ROS RMW API
+ *  @{
+ */
+
+/**
+ * \brief Sets the callback functions for handling error in static memory handling
+ *
+ * \param[in] error_cb callback to be triggered on static memory failure
+ */
+void rmw_uros_set_error_handling_callback(
+  rmw_uros_error_handling error_cb);
+
+/** @}*/
+
+#if defined(__cplusplus)
+}
+#endif  // if defined(__cplusplus)
+
+#endif  // RMW_UROS_ERROR_HANDLING
+
+#endif  // RMW_MICROROS__ERROR_HANDLING_H_

rmw_microxrcedds_c/include/rmw_microros/rmw_microros.h

@@ -34,6 +34,10 @@
 #include <rmw_microros/custom_transport.h>
 #endif  // RMW_UXRCE_TRANSPORT_CUSTOM
 
+#ifdef RMW_UROS_ERROR_HANDLING
+#include <rmw_microros/error_handling.h>
+#endif  // RMW_UROS_ERROR_HANDLING
+
 #if defined(__cplusplus)
 extern "C"
 {

rmw_microxrcedds_c/include/rmw_microros/time_sync.h

@@ -22,7 +22,6 @@
 #include <rmw/rmw.h>
 #include <rmw/ret_types.h>
 #include <rmw/init_options.h>
-#include <rmw/error_handling.h>
 #include <uxr/client/util/time.h>
 
 #if defined(__cplusplus)

rmw_microxrcedds_c/include/rmw_microros/timing.h

@@ -22,7 +22,6 @@
 #include <rmw/rmw.h>
 #include <rmw/ret_types.h>
 #include <rmw/init_options.h>
-#include <rmw/error_handling.h>
 #include <uxr/client/util/time.h>
 
 #if defined(__cplusplus)

rmw_microxrcedds_c/include/rmw_microxrcedds_c/rmw_c_macros.h

@@ -15,11 +15,14 @@
 #ifndef RMW_MICROXRCEDDS_C__RMW_C_MACROS_H_
 #define RMW_MICROXRCEDDS_C__RMW_C_MACROS_H_
 
-#define RMW_CHECK_TYPE_IDENTIFIERS_MATCH(ElementName, ElementTypeID, ExpectedTypeID, OnFailure) \
+#include <rmw/error_handling.h>
+#include <rmw_microros_internal/identifiers.h>
+
+#define RMW_CHECK_TYPE_IDENTIFIERS_MATCH(identifier, ret_on_failure) \
   { \
-    if (strcmp(ElementTypeID, ExpectedTypeID) != 0) { \
+    if (NULL != identifier && strcmp(identifier, eprosima_microxrcedds_identifier) != 0) { \
       RMW_SET_ERROR_MSG("Implementation identifiers does not match"); \
-      OnFailure; \
+      return ret_on_failure; \
     } \
   }
 

rmw_microxrcedds_c/src/callbacks.c

@@ -13,8 +13,7 @@
 // limitations under the License.
 
 #include <rmw_microros_internal/callbacks.h>
-
-#include <rmw/error_handling.h>
+#include "./rmw_microros_internal/error_handling_internal.h"
 
 void on_status(
   struct uxrSession * session,
@@ -75,8 +74,15 @@ void on_topic(
       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");
         UXR_UNLOCK(&static_buffer_memory.mutex);
+        RMW_UROS_TRACE_ERROR(
+          RMW_UROS_ERROR_ON_SUBSCRIPTION, RMW_UROS_ERROR_MIDDLEWARE_ALLOCATION,
+          "Not available static buffer memory node in on_topic callback",
+          .node = custom_subscription->owner_node->node_name,
+          .node_namespace = custom_subscription->owner_node->node_namespace,
+          .topic_name = custom_subscription->topic_name, .ucdr = ub,
+          .size = length,
+          .type_support.message_callbacks = custom_subscription->type_support_callbacks);
         return;
       }
 
@@ -127,8 +133,15 @@ void on_request(
       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");
         UXR_UNLOCK(&static_buffer_memory.mutex);
+        RMW_UROS_TRACE_ERROR(
+          RMW_UROS_ERROR_ON_SERVICE, RMW_UROS_ERROR_MIDDLEWARE_ALLOCATION,
+          "Not available static buffer memory node in on_request callback",
+          .node = custom_service->owner_node->node_name,
+          .node_namespace = custom_service->owner_node->node_namespace,
+          .topic_name = custom_service->service_name, .ucdr = ub,
+          .size = length,
+          .type_support.service_callbacks = custom_service->type_support_callbacks);
         return;
       }
 
@@ -181,8 +194,15 @@ void on_reply(
       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");
         UXR_UNLOCK(&static_buffer_memory.mutex);
+        RMW_UROS_TRACE_ERROR(
+          RMW_UROS_ERROR_ON_CLIENT, RMW_UROS_ERROR_MIDDLEWARE_ALLOCATION,
+          "Not available static buffer memory node in on_reply callback",
+          .node = custom_client->owner_node->node_name,
+          .node_namespace = custom_client->owner_node->node_namespace,
+          .topic_name = custom_client->service_name, .ucdr = ub,
+          .size = length,
+          .type_support.service_callbacks = custom_client->type_support_callbacks);
         return;
       }
 

rmw_microxrcedds_c/src/config.h.in

@@ -12,6 +12,7 @@
 #cmakedefine RMW_UXRCE_USE_REFS
 #cmakedefine RMW_UXRCE_ALLOW_DYNAMIC_ALLOCATIONS
 #cmakedefine RMW_UXRCE_GRAPH
+#cmakedefine RMW_UROS_ERROR_HANDLING
 
 #ifdef RMW_UXRCE_TRANSPORT_UDP
     #define RMW_UXRCE_MAX_TRANSPORT_MTU UXR_CONFIG_UDP_TRANSPORT_MTU

rmw_microxrcedds_c/src/rmw_client.c

@@ -22,9 +22,9 @@
 
 #include <rmw/rmw.h>
 #include <rmw/allocators.h>
-#include <rmw/error_handling.h>
 
 #include "./rmw_microros_internal/utils.h"
+#include "./rmw_microros_internal/error_handling_internal.h"
 
 rmw_client_t *
 rmw_create_client(
@@ -35,20 +35,20 @@ rmw_create_client(
 {
   rmw_client_t * rmw_client = NULL;
   if (!node) {
-    RMW_SET_ERROR_MSG("node handle is null");
+    RMW_UROS_TRACE_MESSAGE("node handle is null")
   } else if (!type_support) {
-    RMW_SET_ERROR_MSG("type support is null");
+    RMW_UROS_TRACE_MESSAGE("type support is null")
   } else if (!is_uxrce_rmw_identifier_valid(node->implementation_identifier)) {
-    RMW_SET_ERROR_MSG("node handle not from this implementation");
+    RMW_UROS_TRACE_MESSAGE("node handle not from this implementation")
   } else if (!service_name || strlen(service_name) == 0) {
-    RMW_SET_ERROR_MSG("service name is null or empty string");
+    RMW_UROS_TRACE_MESSAGE("service name is null or empty string")
   } else if (!qos_policies) {
-    RMW_SET_ERROR_MSG("qos_profile is null");
+    RMW_UROS_TRACE_MESSAGE("qos_profile is null")
   } else {
     rmw_uxrce_node_t * custom_node = (rmw_uxrce_node_t *)node->data;
     rmw_uxrce_mempool_item_t * memory_node = get_memory(&client_memory);
     if (!memory_node) {
-      RMW_SET_ERROR_MSG("Not available memory node");
+      RMW_UROS_TRACE_MESSAGE("Not available memory node")
       return NULL;
     }
     rmw_uxrce_client_t * custom_client = (rmw_uxrce_client_t *)memory_node->data;
@@ -58,7 +58,7 @@ rmw_create_client(
     rmw_client->implementation_identifier = rmw_get_implementation_identifier();
     rmw_client->service_name = custom_client->service_name;
     if ((strlen(service_name) + 1 ) > sizeof(custom_client->service_name)) {
-      RMW_SET_ERROR_MSG("failed to allocate string");
+      RMW_UROS_TRACE_MESSAGE("failed to allocate string")
       goto fail;
     }
 
@@ -80,15 +80,15 @@ rmw_create_client(
     }
 #endif /* ifdef ROSIDL_TYPESUPPORT_MICROXRCEDDS_CPP__IDENTIFIER_VALUE */
     if (NULL == type_support_xrce) {
-      RMW_SET_ERROR_MSG("Undefined type support");
+      RMW_UROS_TRACE_MESSAGE("Undefined type support")
       goto fail;
     }
 
     custom_client->type_support_callbacks =
       (const service_type_support_callbacks_t *)type_support_xrce->data;
 
     if (custom_client->type_support_callbacks == NULL) {
-      RMW_SET_ERROR_MSG("type support data is NULL");
+      RMW_UROS_TRACE_MESSAGE("type support data is NULL")
       goto fail;
     }
 
@@ -109,7 +109,7 @@ rmw_create_client(
         custom_client->type_support_callbacks, qos_policies, rmw_uxrce_entity_naming_buffer,
         sizeof(rmw_uxrce_entity_naming_buffer)))
     {
-      RMW_SET_ERROR_MSG("failed to generate xml request for client creation");
+      RMW_UROS_TRACE_MESSAGE("failed to generate xml request for client creation")
       goto fail;
     }
     client_req = uxr_buffer_create_requester_xml(
@@ -190,22 +190,22 @@ rmw_destroy_client(
 {
   rmw_ret_t result_ret = RMW_RET_OK;
   if (!node) {
-    RMW_SET_ERROR_MSG("node handle is null");
+    RMW_UROS_TRACE_MESSAGE("node handle is null")
     result_ret = RMW_RET_ERROR;
   } else if (!is_uxrce_rmw_identifier_valid(node->implementation_identifier)) {
-    RMW_SET_ERROR_MSG("node handle not from this implementation");
+    RMW_UROS_TRACE_MESSAGE("node handle not from this implementation")
     result_ret = RMW_RET_ERROR;
   } else if (!node->data) {
-    RMW_SET_ERROR_MSG("node imp is null");
+    RMW_UROS_TRACE_MESSAGE("node imp is null")
     result_ret = RMW_RET_ERROR;
   } else if (!client) {
-    RMW_SET_ERROR_MSG("client handle is null");
+    RMW_UROS_TRACE_MESSAGE("client handle is null")
     result_ret = RMW_RET_ERROR;
   } else if (!is_uxrce_rmw_identifier_valid(client->implementation_identifier)) {
-    RMW_SET_ERROR_MSG("client handle not from this implementation");
+    RMW_UROS_TRACE_MESSAGE("client handle not from this implementation")
     result_ret = RMW_RET_ERROR;
   } else if (!client->data) {
-    RMW_SET_ERROR_MSG("client imp is null");
+    RMW_UROS_TRACE_MESSAGE("client imp is null")
     result_ret = RMW_RET_ERROR;
   } else {
     rmw_uxrce_node_t * custom_node = (rmw_uxrce_node_t *)node->data;

rmw_microxrcedds_c/src/rmw_compare_gids_equal.c

@@ -15,6 +15,9 @@
 #include <rmw/rmw.h>
 #include <rmw/error_handling.h>
 
+#include <rmw_microxrcedds_c/rmw_c_macros.h>
+#include "./rmw_microros_internal/error_handling_internal.h"
+
 rmw_ret_t
 rmw_compare_gids_equal(
   const rmw_gid_t * gid1,
@@ -24,13 +27,12 @@ rmw_compare_gids_equal(
   // Check
   RMW_CHECK_ARGUMENT_FOR_NULL(gid1, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_ARGUMENT_FOR_NULL(gid2, RMW_RET_INVALID_ARGUMENT);
-  if (gid1->implementation_identifier != rmw_get_implementation_identifier()) {
-    RMW_SET_ERROR_MSG("publisher handle not from this implementation");
-    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION;
-  } else if (gid2->implementation_identifier != rmw_get_implementation_identifier()) {
-    RMW_SET_ERROR_MSG("publisher handle not from this implementation");
-    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION;
-  }
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    gid1->implementation_identifier,
+    RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    gid2->implementation_identifier,
+    RMW_RET_INCORRECT_RMW_IMPLEMENTATION);
 
   *result = memcmp(gid1->data, gid2->data, sizeof(rmw_gid_t)) == 0;