Add convenience error handling macros (#421)

* Add convenience error handling macros

Signed-off-by: methylDragon <[email protected]>

* Refine error handling macros

Signed-off-by: methylDragon <[email protected]>

* Remove conditional in error handling macros

Signed-off-by: methylDragon <[email protected]>

---------

Signed-off-by: methylDragon <[email protected]>

Author: methylDragon

include/rcutils/error_handling.h

@@ -314,6 +314,75 @@ RCUTILS_PUBLIC
 void
 rcutils_reset_error(void);
 
+/// Set the error message using RCUTILS_SET_ERROR_MSG and append the previous error.
+/**
+ * If there is no previous error, has same behavior as RCUTILS_SET_ERROR_MSG.
+ * \param[in] msg The error message to be set.
+ */
+#define RCUTILS_SET_ERROR_MSG_AND_APPEND_PREV_ERROR(msg) \
+  do { \
+    rcutils_error_string_t error_string = rcutils_get_error_string(); \
+    rcutils_reset_error(); \
+    RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( \
+      RCUTILS_EXPAND(msg ": %s"), error_string.str); \
+  } while (0)
+
+/// Set the error message with RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING and append the previous
+/// error.
+/**
+ * This function sets the error message using the given format string, and appends and resets the
+ * latest error string.
+ * The resulting formatted string is silently truncated at RCUTILS_ERROR_MESSAGE_MAX_LENGTH.
+ *
+ * If there is no previous error, has same behavior as RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING.
+ *
+ * \param[in] format_string The string to be used as the format of the error message.
+ * \param[in] ... Arguments for the format string.
+ */
+#define RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING_AND_APPEND_PREV_ERROR(format_string, ...) \
+  do { \
+    rcutils_error_string_t error_string = rcutils_get_error_string(); \
+    rcutils_reset_error(); \
+    RCUTILS_SET_ERROR_MSG_WITH_FORMAT_STRING( \
+      RCUTILS_EXPAND(format_string ": %s"), __VA_ARGS__, error_string.str); \
+  } while (0)
+
+/// Write the given msg out to stderr, limiting the buffer size in the `fwrite`, appending the
+/// previous error.
+/**
+ * This will reset the previous error, if it exists.
+ * If there is no previous error, has same behavior as RCUTILS_SAFE_FWRITE_TO_STDERR.
+ */
+#define RCUTILS_SAFE_FWRITE_TO_STDERR_AND_APPEND_PREV_ERROR(msg) \
+  do { \
+    rcutils_error_string_t error_string = rcutils_get_error_string(); \
+    rcutils_reset_error(); \
+    RCUTILS_SAFE_FWRITE_TO_STDERR(msg); \
+    RCUTILS_SAFE_FWRITE_TO_STDERR_WITH_FORMAT_STRING(": %s", error_string.str); \
+  } while (0)
+
+/// Set the error message to stderr using a format string and format arguments, appending the
+/// previous error.
+/**
+ * This function sets the error message to stderr using the given format string, appending and
+ * resetting the previous error.
+ * The resulting formatted string is silently truncated at RCUTILS_ERROR_MESSAGE_MAX_LENGTH.
+ *
+ * This will reset the previous error, if it exists.
+ * If there is no previous error, has same behavior as
+ * RCUTILS_SAFE_FWRITE_TO_STDERR_WITH_FORMAT_STRING.
+ *
+ * \param[in] format_string The string to be used as the format of the error message.
+ * \param[in] ... Arguments for the format string.
+ */
+#define RCUTILS_SAFE_FWRITE_TO_STDERR_WITH_FORMAT_STRING_AND_APPEND_PREV_ERROR(format_string, ...) \
+  do { \
+    rcutils_error_string_t error_string = rcutils_get_error_string(); \
+    rcutils_reset_error(); \
+    RCUTILS_SAFE_FWRITE_TO_STDERR_WITH_FORMAT_STRING(format_string, __VA_ARGS__); \
+    RCUTILS_SAFE_FWRITE_TO_STDERR_WITH_FORMAT_STRING(": %s", error_string.str); \
+  } while (0)
+
 #ifdef __cplusplus
 }
 #endif

include/rcutils/macros.h

@@ -64,6 +64,8 @@ extern "C"
   #define RCUTILS_THREAD_LOCAL _Thread_local
 #endif
 
+// Helper macros for nested macro expansion
+#define RCUTILS_EXPAND(x) x
 #define RCUTILS_STRINGIFY_IMPL(x) #x
 #define RCUTILS_STRINGIFY(x) RCUTILS_STRINGIFY_IMPL(x)