feat: add optional colorized output support

Add ros2cli/color.py with an extensible colorizer framework that wraps
sys.stdout and applies ANSI colors line-by-line. Colorizers are
registered per (command, verb) pair; unregistered commands fall back to
DefaultColorizer.

Colorized output is activated when ROS_COLORIZED_OUTPUT=1 and stdout is
a TTY, or via the new --color CLI flag.

Signed-off-by: Peng Wang <penwang@qti.qualcomm.com>

Author: penww

ros2cli/ros2cli/cli.py

@@ -16,11 +16,15 @@
 import argparse
 import builtins
 import functools
+import os
 import signal
 import sys
 
 from rclpy.executors import ExternalShutdownException
 
+from ros2cli.color import ColoringStdout
+from ros2cli.color import get_colorizer
+from ros2cli.color import is_color_enabled
 from ros2cli.command import add_subparsers_on_demand
 
 
@@ -42,6 +46,11 @@ def main(*, script_name='ros2', argv=None, description=None, extension=None):
             'Do not force line buffering in stdout and instead use the python default buffering, '
             'which might be affected by PYTHONUNBUFFERED/-u and depends on whatever stdout is '
             'interactive or not'))
+    parser.add_argument(
+        '--color',
+        action='store_true',
+        default=False,
+        help='Force colorized output (also enabled by ROS_COLORIZED_OUTPUT=1)')
 
     # add arguments for command extension(s)
     if extension:
@@ -81,6 +90,15 @@ def main(*, script_name='ros2', argv=None, description=None, extension=None):
         # get extension identified by the passed command (if available)
         extension = getattr(args, selected_extension_key, None)
 
+    # activate colorized output if requested
+    if args.color:
+        os.environ['ROS_COLORIZED_OUTPUT'] = '1'
+    if is_color_enabled():
+        command_name = getattr(extension, 'NAME', '') if extension else ''
+        verb_name = getattr(getattr(args, '_verb', None), 'NAME', '') or ''
+        colorizer = get_colorizer(command_name, verb_name)
+        sys.stdout = ColoringStdout(sys.stdout, colorizer)
+
     # handle the case that no command was passed
     if extension is None:
         parser.print_help()