Adds {severity_with_color} as a log format option.

Signed-off-by: Alex MacLean <macleale@amazon.com>

Author: Alex MacLean

CMakeLists.txt

@@ -150,6 +150,15 @@ if(BUILD_TESTING)
     TIMEOUT 10
   )
 
+  add_executable(test_logging_severity_with_color test/test_logging_severity_with_color.cpp)
+  target_link_libraries(test_logging_severity_with_color ${PROJECT_NAME})
+  add_launch_test(
+    "test/test_logging_severity_with_color.py"
+    TARGET test_logging_severity_with_color
+    WORKING_DIRECTORY "$<TARGET_FILE_DIR:test_logging_severity_with_color>"
+    TIMEOUT 10
+  )
+
   add_launch_test(
     "test/test_logging_output_format.py"
     TARGET test_logging_output_format

src/logging.c

@@ -381,6 +381,12 @@ static const char * expand_file_name(
   return logging_output->buffer;
 }
 
+// Forward declare expand_serverity_with_color to associate it with tokens.
+static const char * expand_severity_with_color(
+  const logging_input_t * logging_input,
+  rcutils_char_array_t * logging_output,
+  size_t start_offset, size_t end_offset);
+
 typedef struct token_map_entry_s
 {
   const char * token;
@@ -389,6 +395,7 @@ typedef struct token_map_entry_s
 
 static const token_map_entry_t tokens[] = {
   {.token = "severity", .handler = expand_severity},
+  {.token = "severity_with_color", .handler = expand_severity_with_color},
   {.token = "name", .handler = expand_name},
   {.token = "message", .handler = expand_message},
   {.token = "function_name", .handler = expand_function_name},
@@ -1407,13 +1414,24 @@ rcutils_ret_t rcutils_logging_format_message(
 # define SET_STANDARD_COLOR_IN_STREAM(is_colorized, status)
 #endif
 
+static bool rcutils_logging_is_colorized()
+{
+  if (g_colorized_output == RCUTILS_COLORIZED_OUTPUT_FORCE_ENABLE) {
+    return true;
+  }
+  if (g_colorized_output == RCUTILS_COLORIZED_OUTPUT_FORCE_DISABLE) {
+    return false;
+  }
+  return IS_STREAM_A_TTY(g_output_stream);
+}
+
 void rcutils_logging_console_output_handler(
   const rcutils_log_location_t * location,
   int severity, const char * name, rcutils_time_point_value_t timestamp,
   const char * format, va_list * args)
 {
   rcutils_ret_t status = RCUTILS_RET_OK;
-  bool is_colorized = false;
+  bool is_colorized = rcutils_logging_is_colorized();
 
   if (!g_rcutils_logging_initialized) {
     RCUTILS_SAFE_FWRITE_TO_STDERR(
@@ -1434,14 +1452,6 @@ void rcutils_logging_console_output_handler(
       return;
   }
 
-  if (g_colorized_output == RCUTILS_COLORIZED_OUTPUT_FORCE_ENABLE) {
-    is_colorized = true;
-  } else if (g_colorized_output == RCUTILS_COLORIZED_OUTPUT_FORCE_DISABLE) {
-    is_colorized = false;
-  } else {
-    is_colorized = IS_STREAM_A_TTY(g_output_stream);
-  }
-
   char msg_buf[1024] = "";
   rcutils_char_array_t msg_array = {
     .buffer = msg_buf,
@@ -1501,3 +1511,51 @@ void rcutils_logging_console_output_handler(
     RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to fini array.\n");
   }
 }
+
+static const char * expand_severity_with_color(
+  const logging_input_t * logging_input,
+  rcutils_char_array_t * logging_output,
+  size_t start_offset, size_t end_offset)
+{
+  (void)start_offset;
+  (void)end_offset;
+
+  if (rcutils_logging_is_colorized()) {
+    // The entire message is colorized, so we don't need to colorize the severity.
+    return expand_severity(logging_input, logging_output, start_offset, end_offset);
+  }
+
+  rcutils_ret_t status = RCUTILS_RET_OK;
+
+  SET_OUTPUT_COLOR_WITH_SEVERITY(status, logging_input->severity, *logging_output)
+  if (RCUTILS_RET_OK != status) {
+    RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to set color on severity.\n");
+    return NULL;
+  }
+
+  const char * severity_string = g_rcutils_log_severity_names[logging_input->severity];
+  if (rcutils_char_array_strcat(logging_output, severity_string) != RCUTILS_RET_OK) {
+    RCUTILS_SAFE_FWRITE_TO_STDERR(rcutils_get_error_string().str);
+    rcutils_reset_error();
+    RCUTILS_SAFE_FWRITE_TO_STDERR("\n");
+    return NULL;
+  }
+
+  // If the severity is 4 characters long, add another space to line it up with the 5 character severities.
+  if (strlen(severity_string) == 4) {
+    if (rcutils_char_array_strcat(logging_output, " ") != RCUTILS_RET_OK) {
+      RCUTILS_SAFE_FWRITE_TO_STDERR(rcutils_get_error_string().str);
+      rcutils_reset_error();
+      RCUTILS_SAFE_FWRITE_TO_STDERR("\n");
+      return NULL;
+    }
+  }
+
+  SET_STANDARD_COLOR_IN_BUFFER(true, status, *logging_output)
+  if (RCUTILS_RET_OK != status) {
+    RCUTILS_SAFE_FWRITE_TO_STDERR("Failed to reset color after severity.\n");
+    return NULL;
+  }
+
+  return logging_output->buffer;
+}

test/test_logging_severity_with_color.cpp

@@ -0,0 +1,46 @@
+// Copyright 2025 Open Source Robotics Foundation, Inc.
+//
+// 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.
+
+#include <iostream>
+
+#include "rcutils/error_handling.h"
+#include "rcutils/logging.h"
+#include "rcutils/types/rcutils_ret.h"
+
+int main(int, char **)
+{
+  rcutils_ret_t ret = rcutils_logging_initialize();
+  if (ret != RCUTILS_RET_OK) {
+    fprintf(stderr, "error initializing logging: %s\n", rcutils_get_error_string().str);
+    return -1;
+  }
+
+  rcutils_ret_t status = rcutils_logging_set_logger_level("name", RCUTILS_LOG_SEVERITY_DEBUG);
+  if (status != RCUTILS_RET_OK) {
+    fprintf(stderr, "error setting logger level: %s\n", rcutils_get_error_string().str);
+    return -1;
+  }
+
+  // Log at all 5 severities to check the colorized severity in the log output.
+  rcutils_log_location_t location = {"func", "file", 42u};
+  rcutils_log(&location, RCUTILS_LOG_SEVERITY_DEBUG, "name", "Debug message");
+  rcutils_log(&location, RCUTILS_LOG_SEVERITY_INFO, "name", "Info message");
+  rcutils_log(&location, RCUTILS_LOG_SEVERITY_WARN, "name", "Warn message");
+  rcutils_log(&location, RCUTILS_LOG_SEVERITY_ERROR, "name", "Error message");
+  rcutils_log(&location, RCUTILS_LOG_SEVERITY_FATAL, "name", "Fatal message");
+
+  std::cout.flush();
+
+  return 0;
+}

test/test_logging_severity_with_color.py

@@ -0,0 +1,84 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# 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.
+
+import os
+import unittest
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+from launch.actions import SetEnvironmentVariable
+
+import launch_testing
+import launch_testing.actions
+import launch_testing.asserts
+import launch_testing.markers
+
+
+@launch_testing.markers.keep_alive
+def generate_test_description():
+    test_process_name='test_logging_severity_with_color'
+    launch_description = LaunchDescription()
+    # Set the output format to a "verbose" format that is expected by the executable output
+    launch_description.add_action(
+        SetEnvironmentVariable(
+            name='RCUTILS_CONSOLE_OUTPUT_FORMAT',
+            value='[{severity_with_color}] [{name}]: {message} '
+                  '({function_name}() at {file_name}:{line_number})'
+        )
+    )
+    launch_description.add_action(
+        SetEnvironmentVariable(
+            name='RCUTILS_COLORIZED_OUTPUT',
+            value='0'
+        )
+    )
+    executable = os.path.join(os.getcwd(), test_process_name)
+    if os.name == 'nt':
+        executable += '.exe'
+    process_name = test_process_name
+    launch_description.add_action(
+        ExecuteProcess(cmd=[executable], name=process_name, output='screen')
+    )
+
+    launch_description.add_action(
+        launch_testing.actions.ReadyToTest()
+    )
+    return launch_description, {'process_name': process_name}
+
+
+class TestLoggingSeverityWithColor(unittest.TestCase):
+
+    def test_wait_for_shutdown(self, proc_info, proc_output, process_name):
+        """Wait for the process to complete so that the log messages will be available to inspect."""
+        proc_info.assertWaitForShutdown(process=process_name, timeout=10)
+
+
+@launch_testing.post_shutdown_test()
+class TestLoggingSeverityWithColorAfterShutdown(unittest.TestCase):
+
+    def test_logging_output(self, proc_output, process_name):
+        """Test executable output against expectation."""
+        launch_testing.asserts.assertInStderr(
+            proc_output,
+            expected_output=launch_testing.tools.expected_output_from_file(
+                path=os.path.join(os.path.dirname(__file__), process_name),
+                encoding='unicode_escape'
+            ),
+            process=process_name,
+            strip_ansi_escape_sequences=False
+        )
+
+    def test_processes_exit_codes(self, proc_info):
+        """Test that all executables finished cleanly."""
+        launch_testing.asserts.assertExitCodes(proc_info)

test/test_logging_severity_with_color.txt

@@ -0,0 +1,5 @@
+[\033[32mDEBUG\033[0m] [name]: Debug message (func() at file:42)
+[\033[0mINFO \033[0m] [name]: Info message (func() at file:42)
+[\033[33mWARN \033[0m] [name]: Warn message (func() at file:42)
+[\033[31mERROR\033[0m] [name]: Error message (func() at file:42)
+[\033[31mFATAL\033[0m] [name]: Fatal message (func() at file:42)