Allow parsing of escape sequence in log format (#443)

Signed-off-by: Marc Bestmann <marc.bestmann@dlr.de>

Author: SammyRamone

src/logging.c

@@ -87,6 +87,10 @@ bool g_rcutils_logging_initialized = false;
 static char g_rcutils_logging_output_format_string[RCUTILS_LOGGING_MAX_OUTPUT_FORMAT_LEN];
 static const char * g_rcutils_logging_default_output_format =
   "[{severity}] [{time}] [{name}]: {message}";
+#ifdef _WIN32
+static DWORD g_original_console_mode = 0;
+static bool g_consol_mode_modified = false;
+#endif
 
 static rcutils_allocator_t g_rcutils_logging_allocator;
 
@@ -415,6 +419,30 @@ static const char * copy_from_orig(
   return logging_output->buffer;
 }
 
+#ifdef _WIN32
+#define ACTIVATE_VIRTUAL_TERMINAL_PROCESSING() \
+  { \
+    HANDLE std_error_handle = GetStdHandle(STD_ERROR_HANDLE); \
+    if (std_error_handle == INVALID_HANDLE_VALUE) { \
+      RCUTILS_SET_ERROR_MSG("Could not get error handle to activating virtual terminal."); \
+      return; \
+    } \
+    if (!GetConsoleMode(std_error_handle, &g_original_console_mode)) { \
+      RCUTILS_SET_ERROR_MSG("Could not get consol mode to activating virtual terminal."); \
+      return; \
+    } \
+    DWORD newDwMode = g_original_console_mode | ENABLE_VIRTUAL_TERMINAL_PROCESSING; \
+    if (!SetConsoleMode(std_error_handle, newDwMode)) { \
+      RCUTILS_SET_ERROR_MSG("Could not set consol mode to activating virtual terminal."); \
+      return; \
+    } \
+    g_consol_mode_modified = true; \
+  }
+#else
+// nothing todo for non-windows platform
+#define ACTIVATE_VIRTUAL_TERMINAL_PROCESSING()
+#endif
+
 // copy buffers and decode escape characters if they exist
 static void create_format_string(
   const char * logging_output_format_string)
@@ -440,14 +468,28 @@ static void create_format_string(
       break;
     } else {
       const char * expected_char = NULL;
-      switch (logging_output_format_string[i + back_slash_index + 1]) {
-        case 'a':  expected_char = "\a"; break;  // alert
-        case 'b':  expected_char = "\b"; break;  // backspace
-        case 'n':  expected_char = "\n"; break;  // new line
-        case 'r':  expected_char = "\r"; break;  // carriage return
-        case 't':  expected_char = "\t"; break;  // horizontal tab
-        default:
-          break;
+      int skip_chars = 0;
+
+      if (logging_output_format_string[i + back_slash_index + 1] == 'x' &&
+        logging_output_format_string[i + back_slash_index + 2] == '1' &&
+        logging_output_format_string[i + back_slash_index + 3] == 'b')
+      {
+        // detect escape sequence
+        ACTIVATE_VIRTUAL_TERMINAL_PROCESSING();
+        expected_char = "\x1b";
+        // the 4 char long "\x1b" string literal will become a 2 char long \x1b escape sequence
+        // therefore we need to skip forward in parsing the output format string
+        skip_chars = 2;
+      } else {
+        switch (logging_output_format_string[i + back_slash_index + 1]) {
+          case 'a':  expected_char = "\a"; break;  // alert
+          case 'b':  expected_char = "\b"; break;  // backspace
+          case 'n':  expected_char = "\n"; break;  // new line
+          case 'r':  expected_char = "\r"; break;  // carriage return
+          case 't':  expected_char = "\t"; break;  // horizontal tab
+          default:
+            break;
+        }
       }
 
       if (expected_char) {
@@ -466,12 +508,12 @@ static void create_format_string(
         // copy the decoded character
         g_rcutils_logging_output_format_string[dest_buffer_index] = expected_char[0];
         dest_buffer_index += 1;
-        start_offset += 2;
+        start_offset += 2 + skip_chars;
       } else {
         start_offset_previous_not_copy += (back_slash_index + 2);
       }
 
-      i += (back_slash_index + 2);
+      i += (back_slash_index + 2 + skip_chars);
     }
   }
 }
@@ -749,6 +791,12 @@ rcutils_ret_t rcutils_logging_shutdown(void)
   }
   g_num_log_msg_handlers = 0;
   g_rcutils_logging_initialized = false;
+
+  #ifdef _WIN32
+  if (g_consol_mode_modified) {
+    SetConsoleMode(GetStdHandle(STD_ERROR_HANDLE), g_original_console_mode);
+  }
+  #endif
   return ret;
 }
 

test/test_logging_output_format.py

@@ -91,6 +91,16 @@ def generate_test_description():
     ))
     processes_to_test.append(name)
 
+    env_escape_sequence = dict(os.environ)
+    # This custom output is to check that escape characters work correctly.
+    env_escape_sequence['RCUTILS_CONSOLE_OUTPUT_FORMAT'] = \
+        '{name} \x1b[0m {message}'
+    name = 'test_logging_output_format_escape_sequence'
+    launch_description.add_action(ExecuteProcess(
+        cmd=[executable], env=env_escape_sequence, name=name, output='screen'
+    ))
+    processes_to_test.append(name)
+
     launch_description.add_action(
         launch_testing.actions.ReadyToTest()
     )
@@ -117,7 +127,8 @@ def test_logging_output(self, proc_output, processes_to_test):
                     path=os.path.join(os.path.dirname(__file__), process_name),
                     encoding='unicode_escape'
                 ),
-                process=process_name
+                process=process_name,
+                strip_ansi_escape_sequences=False
             )
 
     def test_processes_exit_codes(self, proc_info):

test/test_logging_output_format_escape_sequence.regex

@@ -0,0 +1,2 @@
+name1 \x1b\[0m Xx{2045}X
+name2 \x1b\[0m X42x{2043}X