add content filter for action client feedback subscription

Signed-off-by: Chen Lihui <[email protected]>

rcl/include/rcl/macros.h

@@ -27,6 +27,37 @@ extern "C"
 
 #define RCL_UNUSED(x) RCUTILS_UNUSED(x)
 
+#define RCL_RET_FROM_RCUTIL_RET(rcl_ret_var, rcutils_expr) \
+  { \
+    rcutils_ret_t rcutils_ret = rcutils_expr; \
+    if (RCUTILS_RET_OK != rcutils_ret) { \
+      if (rcutils_error_is_set()) { \
+        RCL_SET_ERROR_MSG(rcutils_get_error_string().str); \
+      } else { \
+        RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("rcutils_ret_t code: %i", rcutils_ret); \
+      } \
+    } \
+    switch (rcutils_ret) { \
+      case RCUTILS_RET_OK: \
+        rcl_ret_var = RCL_RET_OK; \
+        break; \
+      case RCUTILS_RET_ERROR: \
+        rcl_ret_var = RCL_RET_ERROR; \
+        break; \
+      case RCUTILS_RET_BAD_ALLOC: \
+        rcl_ret_var = RCL_RET_BAD_ALLOC; \
+        break; \
+      case RCUTILS_RET_INVALID_ARGUMENT: \
+        rcl_ret_var = RCL_RET_INVALID_ARGUMENT; \
+        break; \
+      case RCUTILS_RET_NOT_INITIALIZED: \
+        rcl_ret_var = RCL_RET_NOT_INIT; \
+        break; \
+      default: \
+        rcl_ret_var = RCUTILS_RET_ERROR; \
+    } \
+  }
+
 #ifdef __cplusplus
 }
 #endif

rcl/src/rcl/logging_rosout.c

@@ -15,6 +15,7 @@
 #include "rcl/allocator.h"
 #include "rcl/error_handling.h"
 #include "rcl/logging_rosout.h"
+#include "rcl/macros.h"
 #include "rcl/node.h"
 #include "rcl/publisher.h"
 #include "rcl/time.h"
@@ -43,37 +44,6 @@ extern "C"
     return RCL_RET_OK; \
   }
 
-#define RCL_RET_FROM_RCUTIL_RET(rcl_ret_var, rcutils_expr) \
-  { \
-    rcutils_ret_t rcutils_ret = rcutils_expr; \
-    if (RCUTILS_RET_OK != rcutils_ret) { \
-      if (rcutils_error_is_set()) { \
-        RCL_SET_ERROR_MSG(rcutils_get_error_string().str); \
-      } else { \
-        RCL_SET_ERROR_MSG_WITH_FORMAT_STRING("rcutils_ret_t code: %i", rcutils_ret); \
-      } \
-    } \
-    switch (rcutils_ret) { \
-      case RCUTILS_RET_OK: \
-        rcl_ret_var = RCL_RET_OK; \
-        break; \
-      case RCUTILS_RET_ERROR: \
-        rcl_ret_var = RCL_RET_ERROR; \
-        break; \
-      case RCUTILS_RET_BAD_ALLOC: \
-        rcl_ret_var = RCL_RET_BAD_ALLOC; \
-        break; \
-      case RCUTILS_RET_INVALID_ARGUMENT: \
-        rcl_ret_var = RCL_RET_INVALID_ARGUMENT; \
-        break; \
-      case RCUTILS_RET_NOT_INITIALIZED: \
-        rcl_ret_var = RCL_RET_NOT_INIT; \
-        break; \
-      default: \
-        rcl_ret_var = RCUTILS_RET_ERROR; \
-    } \
-  }
-
 typedef struct rosout_map_entry_t
 {
   rcl_node_t * node;

rcl_action/include/rcl_action/action_client.h

@@ -24,6 +24,7 @@ extern "C"
 #include "rcl_action/visibility_control.h"
 #include "rcl/macros.h"
 #include "rcl/node.h"
+#include "rcl/subscription.h"
 
 
 /// Internal action client implementation struct.
@@ -741,6 +742,64 @@ bool
 rcl_action_client_is_valid(
   const rcl_action_client_t * action_client);
 
+/// Add a goal uuid.
+/**
+ * This function is to add a goal uuid to the map of rcl_action_client_t
+ * and then try to set content filtered topic if it is supported.
+ *
+ * The caller must provide a lock to call this interface
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] action_client handle to the client that will take the goal response
+ * \param[in] uuid pointer to a uuid which length is 16
+ * \return `RCL_RET_OK` if success on setting a goal uuid, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_ACTION_CLIENT_INVALID` if the action client is invalid, or
+ * \return `RCL_RET_UNSUPPORTED` if setting content filtered topic is not supported
+ *         in the middleware, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_ACTION_PUBLIC
+rcl_ret_t rcl_action_add_goal_uuid(
+  const rcl_action_client_t * action_client,
+  const uint8_t * uuid);
+
+/// Remove a goal uuid.
+/**
+ * This function is to remove a goal uuid from the map of rcl_action_client_t
+ * and then try to reset content filtered topic if it is supported.
+ *
+ * The caller must provide a lock to call this interface
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No
+ * Thread-Safe        | No
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ *
+ * \param[in] action_client handle to the client that will take the goal response
+ * \param[in] uuid pointer to a uuid which length is 16
+ * \return `RCL_RET_OK` if success on removing a goal uuid, or
+ * \return `RCL_RET_INVALID_ARGUMENT` if any arguments are invalid, or
+ * \return `RCL_RET_ACTION_CLIENT_INVALID` if the action client is invalid, or
+ * \return `RCL_RET_UNSUPPORTED` if setting content filtered topic is not supported
+ *         in the middleware, or
+ * \return `RCL_RET_ERROR` if an unspecified error occurs.
+ */
+RCL_ACTION_PUBLIC
+rcl_ret_t rcl_action_remove_goal_uuid(
+  const rcl_action_client_t * action_client,
+  const uint8_t * uuid);
+
 #ifdef __cplusplus
 }
 #endif

rcl_action/include/rcl_action/types.h

@@ -58,6 +58,25 @@ extern "C"
 #define uuidcmp(uuid0, uuid1) (0 == memcmp(uuid0, uuid1, UUID_SIZE))
 #define zerouuid (uint8_t[UUID_SIZE]) {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}
 #define uuidcmpzero(uuid) uuidcmp(uuid, (zerouuid))
+#define UUID_FMT \
+  "%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X%02X"
+#define UUID_FMT_ARGS(uuid) \
+  uuid[0], \
+  uuid[1], \
+  uuid[2], \
+  uuid[3], \
+  uuid[4], \
+  uuid[5], \
+  uuid[6], \
+  uuid[7], \
+  uuid[8], \
+  uuid[9], \
+  uuid[10], \
+  uuid[11], \
+  uuid[12], \
+  uuid[13], \
+  uuid[14], \
+  uuid[15]
 
 // Typedef generated messages for convenience
 typedef action_msgs__msg__GoalInfo rcl_action_goal_info_t;