From db155115801931fa66007b072f1b798a6e8e9ff1 Mon Sep 17 00:00:00 2001 From: sauman raaj <83863464+saumanraaj@users.noreply.github.com> Date: Fri, 24 Oct 2025 20:10:07 -0400 Subject: [PATCH 1/4] Fix ANSI escape code pollution in log output - Make bcolors class environment-aware to respect RCUTILS_COLORIZED_OUTPUT - Remove ANSI codes from all logger calls in controller_manager_services.py - Remove ANSI codes from logger calls in spawner.py and hardware_spawner.py - Maintain backward compatibility with existing bcolors usage - Fixes Issue #2592: Option for uncolored log output Environment variable behavior: - RCUTILS_COLORIZED_OUTPUT=0: No colors in output - RCUTILS_COLORIZED_OUTPUT=1: Colors enabled - Unset: Auto-detect TTY for color support --- .../controller_manager_services.py | 82 +++++++++++-------- .../controller_manager/hardware_spawner.py | 22 +++-- .../controller_manager/spawner.py | 63 ++++---------- 3 files changed, 80 insertions(+), 87 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index dc2647993f..3f3ddd0175 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -38,17 +38,33 @@ from ros2param.api import call_set_parameters -# from https://stackoverflow.com/a/287944 +import os +import sys + + +def _color_enabled(): + """Respect RCUTILS_COLORIZED_OUTPUT: 0=off, 1=on, unset=auto-detect TTY.""" + env = os.getenv("RCUTILS_COLORIZED_OUTPUT") + if env == "0": + return False + if env == "1": + return True + return sys.stdout.isatty() + + +COLOR_ENABLED = _color_enabled() + + class bcolors: - MAGENTA = "\033[95m" - OKBLUE = "\033[94m" - OKCYAN = "\033[96m" - OKGREEN = "\033[92m" - WARNING = "\033[93m" - FAIL = "\033[91m" - ENDC = "\033[0m" - BOLD = "\033[1m" - UNDERLINE = "\033[4m" + MAGENTA = "\033[95m" if COLOR_ENABLED else "" + OKBLUE = "\033[94m" if COLOR_ENABLED else "" + OKCYAN = "\033[96m" if COLOR_ENABLED else "" + OKGREEN = "\033[92m" if COLOR_ENABLED else "" + WARNING = "\033[93m" if COLOR_ENABLED else "" + FAIL = "\033[91m" if COLOR_ENABLED else "" + ENDC = "\033[0m" if COLOR_ENABLED else "" + BOLD = "\033[1m" if COLOR_ENABLED else "" + UNDERLINE = "\033[4m" if COLOR_ENABLED else "" class ServiceNotFoundError(Exception): @@ -75,11 +91,15 @@ def __new__(cls, node, service_type, fully_qualified_service_name): service_type, fully_qualified_service_name ) node.get_logger().debug( - f"{bcolors.MAGENTA}Creating a new service client : {fully_qualified_service_name} with node : {node.get_name()}{bcolors.ENDC}" + "Creating a new service client : %s with node : %s", + fully_qualified_service_name, + node.get_name(), ) node.get_logger().debug( - f"{bcolors.OKBLUE}Returning the existing service client : {fully_qualified_service_name} for node : {node.get_name()}{bcolors.ENDC}" + "Returning the existing service client : %s for node : %s", + fully_qualified_service_name, + node.get_name(), ) return cls._clients[(node, fully_qualified_service_name)] @@ -336,7 +356,9 @@ def get_params_files_with_controller_parameters( if key in parameters: if key == controller_name and namespace != "/": node.get_logger().fatal( - f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}" + "Missing namespace : %s or wildcard in parameter file for controller : %s", + namespace, + controller_name, ) break controller_parameter_files.append(parameter_file) @@ -369,7 +391,9 @@ def get_parameter_from_param_files( if key in parameters: if key == controller_name and namespace != "/": node.get_logger().fatal( - f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}" + "Missing namespace : %s or wildcard in parameter file for controller : %s", + namespace, + controller_name, ) break controller_param_dict = parameters[key] @@ -397,7 +421,9 @@ def get_parameter_from_param_files( return controller_param_dict[ROS_PARAMS_KEY][parameter_name] if controller_param_dict is None: node.get_logger().fatal( - f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter files : {parameter_files}{bcolors.ENDC}" + "Controller : %s parameters not found in parameter files : %s", + namespaced_controller, + parameter_files, ) return None @@ -417,27 +443,17 @@ def set_controller_parameters( result = response.results[0] if result.successful: node.get_logger().info( - bcolors.OKCYAN - + 'Setting controller param "' - + parameter_name - + '" to "' - + parameter_string - + '" for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC + 'Setting controller param "%s" to "%s" for %s', + parameter_name, + parameter_string, + controller_name, ) else: node.get_logger().fatal( - bcolors.FAIL - + 'Could not set controller param "' - + parameter_name - + '" to "' - + parameter_string - + '" for ' - + bcolors.BOLD - + controller_name - + bcolors.ENDC + 'Could not set controller param "%s" to "%s" for %s', + parameter_name, + parameter_string, + controller_name, ) return False return True diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index abba851432..123ea60dcb 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -19,7 +19,6 @@ from controller_manager import ( list_hardware_components, set_hardware_component_state, - bcolors, ) from controller_manager.controller_manager_services import ServiceNotFoundError @@ -67,15 +66,24 @@ def handle_set_component_state_service_call( response = set_hardware_component_state(node, controller_manager_name, component, target_state) if response.ok and response.state == target_state: node.get_logger().info( - f"{bcolors.OKGREEN}{action} component '{component}'. Hardware now in state: {response.state}.{bcolors.ENDC}" + "%s component '%s'. Hardware now in state: %s.", + action, + component, + response.state, ) elif response.ok and not response.state == target_state: node.get_logger().warning( - f"{bcolors.WARNING}Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'.{bcolors.ENDC}" + "Could not %s component '%s'. Service call returned ok=True, but state: %s is not equal to target state '%s'.", + action, + component, + response.state, + target_state, ) else: node.get_logger().warning( - f"{bcolors.WARNING}Could not {action} component '{component}'. Service call failed. Wrong component name?{bcolors.ENDC}" + "Could not %s component '%s'. Service call failed. Wrong component name?", + action, + component, ) @@ -157,7 +165,7 @@ def main(args=None): node, controller_manager_name, hardware_component, controller_manager_timeout ): node.get_logger().warning( - f"{bcolors.WARNING}Hardware Component is not loaded - state can not be changed.{bcolors.ENDC}" + "Hardware Component is not loaded - state can not be changed." ) elif activate: activate_component(node, controller_manager_name, hardware_component) @@ -165,14 +173,14 @@ def main(args=None): configure_component(node, controller_manager_name, hardware_component) else: node.get_logger().error( - f'{bcolors.FAIL}You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag{bcolors.ENDC}' + 'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag' ) parser.print_help() return 0 except KeyboardInterrupt: pass except ServiceNotFoundError as err: - node.get_logger().fatal(f"{bcolors.FAIL}{str(err)}{bcolors.ENDC}") + node.get_logger().fatal(str(err)) return 1 finally: rclpy.shutdown() diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 38628d0af8..de675387d4 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -28,7 +28,6 @@ unload_controller, set_controller_parameters, set_controller_parameters_from_param_files, - bcolors, ) from controller_manager_msgs.srv import SwitchController from controller_manager.controller_manager_services import ServiceNotFoundError @@ -191,24 +190,20 @@ def main(args=None): retry_delay = 3 # seconds for attempt in range(max_retries): try: - logger.debug( - bcolors.OKGREEN + "Waiting for the spawner lock to be acquired!" + bcolors.ENDC - ) + logger.debug("Waiting for the spawner lock to be acquired!") # timeout after 20 seconds and try again lock.acquire(timeout=20) - logger.debug(bcolors.OKGREEN + "Spawner lock acquired!" + bcolors.ENDC) + logger.debug("Spawner lock acquired!") break except Timeout: logger.warning( - bcolors.WARNING - + f"Attempt {attempt+1} failed. Retrying in {retry_delay} seconds..." - + bcolors.ENDC + "Attempt %d failed. Retrying in %d seconds...", + attempt + 1, + retry_delay, ) time.sleep(retry_delay) else: - logger.error( - bcolors.FAIL + "Failed to acquire lock after multiple attempts." + bcolors.ENDC - ) + logger.error("Failed to acquire lock after multiple attempts.") return 1 node = Node(spawner_node_name) @@ -234,11 +229,7 @@ def main(args=None): controller_manager_timeout, service_call_timeout, ): - logger.warning( - bcolors.WARNING - + "Controller already loaded, skipping load_controller" - + bcolors.ENDC - ) + logger.warning("Controller already loaded, skipping load_controller") else: if controller_ros_args := args.controller_ros_args: if not set_controller_parameters( @@ -267,17 +258,9 @@ def main(args=None): service_call_timeout, ) if not ret.ok: - logger.fatal( - bcolors.FAIL - + "Failed loading controller " - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) + logger.fatal("Failed loading controller %s", controller_name) return 1 - logger.info( - bcolors.OKBLUE + "Loaded " + bcolors.BOLD + controller_name + bcolors.ENDC - ) + logger.info("Loaded %s", controller_name) if not args.load_only: ret = configure_controller( @@ -288,7 +271,7 @@ def main(args=None): service_call_timeout, ) if not ret.ok: - logger.error(bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC) + logger.error("Failed to configure controller") return 1 if not args.inactive and not args.activate_as_group: @@ -303,18 +286,10 @@ def main(args=None): service_call_timeout, ) if not ret.ok: - logger.error( - f"{bcolors.FAIL}Failed to activate controller : {controller_name}{bcolors.ENDC}" - ) + logger.error("Failed to activate controller : %s", controller_name) return 1 - logger.info( - bcolors.OKGREEN - + "Configured and activated " - + bcolors.BOLD - + controller_name - + bcolors.ENDC - ) + logger.info("Configured and activated %s", controller_name) if not args.inactive and args.activate_as_group: ret = switch_controllers( @@ -329,14 +304,12 @@ def main(args=None): ) if not ret.ok: logger.error( - f"{bcolors.FAIL}Failed to activate the parsed controllers list : {controller_names}{bcolors.ENDC}" + "Failed to activate the parsed controllers list : %s", controller_names ) return 1 logger.info( - bcolors.OKGREEN - + f"Configured and activated all the parsed controllers list : {controller_names}!" - + bcolors.ENDC + "Configured and activated all the parsed controllers list : %s!", controller_names ) unload_controllers_upon_exit = args.unload_on_kill if not unload_controllers_upon_exit: @@ -364,7 +337,7 @@ def main(args=None): service_call_timeout, ) if not ret.ok: - logger.error(bcolors.FAIL + "Failed to deactivate controller" + bcolors.ENDC) + logger.error("Failed to deactivate controller") return 1 logger.info(f"Successfully deactivated controllers : {controller_names}") @@ -380,11 +353,7 @@ def main(args=None): ) if not ret.ok: unload_status = False - logger.error( - bcolors.FAIL - + f"Failed to unload controller : {controller_name}" - + bcolors.ENDC - ) + logger.error("Failed to unload controller : %s", controller_name) if unload_status: logger.info(f"Successfully unloaded controllers : {controller_names}") From eb90d06f8838810cd04c8d9b626ba46a4f3d5d8d Mon Sep 17 00:00:00 2001 From: sauman raaj <83863464+saumanraaj@users.noreply.github.com> Date: Sat, 25 Oct 2025 11:17:22 -0400 Subject: [PATCH 2/4] Fix Issue #2592: Make bcolors environment-aware - Add _color_enabled() function that respects RCUTILS_COLORIZED_OUTPUT - Make bcolors class environment-aware using _color_enabled() - Remove redundant COLOR_ENABLED variable for cleaner implementation - Add bcolors import to spawner.py and hardware_spawner.py - Fixes ANSI escape code pollution in CI logs while preserving colors in interactive terminals Resolves: #2592 --- .../controller_manager_services.py | 67 +++++++++---------- .../controller_manager/hardware_spawner.py | 34 +++++----- .../controller_manager/spawner.py | 67 ++++++++++++++----- 3 files changed, 97 insertions(+), 71 deletions(-) diff --git a/controller_manager/controller_manager/controller_manager_services.py b/controller_manager/controller_manager/controller_manager_services.py index 3f3ddd0175..9239dee271 100644 --- a/controller_manager/controller_manager/controller_manager_services.py +++ b/controller_manager/controller_manager/controller_manager_services.py @@ -52,19 +52,16 @@ def _color_enabled(): return sys.stdout.isatty() -COLOR_ENABLED = _color_enabled() - - class bcolors: - MAGENTA = "\033[95m" if COLOR_ENABLED else "" - OKBLUE = "\033[94m" if COLOR_ENABLED else "" - OKCYAN = "\033[96m" if COLOR_ENABLED else "" - OKGREEN = "\033[92m" if COLOR_ENABLED else "" - WARNING = "\033[93m" if COLOR_ENABLED else "" - FAIL = "\033[91m" if COLOR_ENABLED else "" - ENDC = "\033[0m" if COLOR_ENABLED else "" - BOLD = "\033[1m" if COLOR_ENABLED else "" - UNDERLINE = "\033[4m" if COLOR_ENABLED else "" + MAGENTA = "\033[95m" if _color_enabled() else "" + OKBLUE = "\033[94m" if _color_enabled() else "" + OKCYAN = "\033[96m" if _color_enabled() else "" + OKGREEN = "\033[92m" if _color_enabled() else "" + WARNING = "\033[93m" if _color_enabled() else "" + FAIL = "\033[91m" if _color_enabled() else "" + ENDC = "\033[0m" if _color_enabled() else "" + BOLD = "\033[1m" if _color_enabled() else "" + UNDERLINE = "\033[4m" if _color_enabled() else "" class ServiceNotFoundError(Exception): @@ -91,15 +88,11 @@ def __new__(cls, node, service_type, fully_qualified_service_name): service_type, fully_qualified_service_name ) node.get_logger().debug( - "Creating a new service client : %s with node : %s", - fully_qualified_service_name, - node.get_name(), + f"{bcolors.MAGENTA}Creating a new service client : {fully_qualified_service_name} with node : {node.get_name()}{bcolors.ENDC}" ) node.get_logger().debug( - "Returning the existing service client : %s for node : %s", - fully_qualified_service_name, - node.get_name(), + f"{bcolors.OKBLUE}Returning the existing service client : {fully_qualified_service_name} for node : {node.get_name()}{bcolors.ENDC}" ) return cls._clients[(node, fully_qualified_service_name)] @@ -356,9 +349,7 @@ def get_params_files_with_controller_parameters( if key in parameters: if key == controller_name and namespace != "/": node.get_logger().fatal( - "Missing namespace : %s or wildcard in parameter file for controller : %s", - namespace, - controller_name, + f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}" ) break controller_parameter_files.append(parameter_file) @@ -391,9 +382,7 @@ def get_parameter_from_param_files( if key in parameters: if key == controller_name and namespace != "/": node.get_logger().fatal( - "Missing namespace : %s or wildcard in parameter file for controller : %s", - namespace, - controller_name, + f"{bcolors.FAIL}Missing namespace : {namespace} or wildcard in parameter file for controller : {controller_name}{bcolors.ENDC}" ) break controller_param_dict = parameters[key] @@ -421,9 +410,7 @@ def get_parameter_from_param_files( return controller_param_dict[ROS_PARAMS_KEY][parameter_name] if controller_param_dict is None: node.get_logger().fatal( - "Controller : %s parameters not found in parameter files : %s", - namespaced_controller, - parameter_files, + f"{bcolors.FAIL}Controller : {namespaced_controller} parameters not found in parameter files : {parameter_files}{bcolors.ENDC}" ) return None @@ -443,17 +430,27 @@ def set_controller_parameters( result = response.results[0] if result.successful: node.get_logger().info( - 'Setting controller param "%s" to "%s" for %s', - parameter_name, - parameter_string, - controller_name, + bcolors.OKCYAN + + 'Setting controller param "' + + parameter_name + + '" to "' + + parameter_string + + '" for ' + + bcolors.BOLD + + controller_name + + bcolors.ENDC ) else: node.get_logger().fatal( - 'Could not set controller param "%s" to "%s" for %s', - parameter_name, - parameter_string, - controller_name, + bcolors.FAIL + + 'Could not set controller param "' + + parameter_name + + '" to "' + + parameter_string + + '" for ' + + bcolors.BOLD + + controller_name + + bcolors.ENDC ) return False return True diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index 123ea60dcb..07df80af02 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -20,7 +20,7 @@ list_hardware_components, set_hardware_component_state, ) -from controller_manager.controller_manager_services import ServiceNotFoundError +from controller_manager.controller_manager_services import ServiceNotFoundError, bcolors from lifecycle_msgs.msg import State import rclpy @@ -66,24 +66,15 @@ def handle_set_component_state_service_call( response = set_hardware_component_state(node, controller_manager_name, component, target_state) if response.ok and response.state == target_state: node.get_logger().info( - "%s component '%s'. Hardware now in state: %s.", - action, - component, - response.state, + f"{bcolors.OKGREEN}{action} component '{component}'. Hardware now in state: {response.state}.{bcolors.ENDC}" ) elif response.ok and not response.state == target_state: node.get_logger().warning( - "Could not %s component '%s'. Service call returned ok=True, but state: %s is not equal to target state '%s'.", - action, - component, - response.state, - target_state, + f"{bcolors.WARNING}Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'.{bcolors.ENDC}" ) else: node.get_logger().warning( - "Could not %s component '%s'. Service call failed. Wrong component name?", - action, - component, + f"{bcolors.WARNING}Could not {action} component '{component}'. Service call failed. Wrong component name?{bcolors.ENDC}" ) @@ -101,7 +92,11 @@ def configure_component(node, controller_manager_name, component_to_configure): inactive_state.id = State.PRIMARY_STATE_INACTIVE inactive_state.label = "inactive" handle_set_component_state_service_call( - node, controller_manager_name, component_to_configure, inactive_state, "configured" + node, + controller_manager_name, + component_to_configure, + inactive_state, + "configured", ) @@ -162,10 +157,13 @@ def main(args=None): try: for hardware_component in hardware_components: if not is_hardware_component_loaded( - node, controller_manager_name, hardware_component, controller_manager_timeout + node, + controller_manager_name, + hardware_component, + controller_manager_timeout, ): node.get_logger().warning( - "Hardware Component is not loaded - state can not be changed." + f"{bcolors.WARNING}Hardware Component is not loaded - state can not be changed.{bcolors.ENDC}" ) elif activate: activate_component(node, controller_manager_name, hardware_component) @@ -173,14 +171,14 @@ def main(args=None): configure_component(node, controller_manager_name, hardware_component) else: node.get_logger().error( - 'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag' + f'{bcolors.FAIL}You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag{bcolors.ENDC}' ) parser.print_help() return 0 except KeyboardInterrupt: pass except ServiceNotFoundError as err: - node.get_logger().fatal(str(err)) + node.get_logger().fatal(f"{bcolors.FAIL}{str(err)}{bcolors.ENDC}") return 1 finally: rclpy.shutdown() diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index de675387d4..e3fdb1f128 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -30,7 +30,7 @@ set_controller_parameters_from_param_files, ) from controller_manager_msgs.srv import SwitchController -from controller_manager.controller_manager_services import ServiceNotFoundError +from controller_manager.controller_manager_services import ServiceNotFoundError, bcolors from filelock import Timeout, FileLock import rclpy @@ -50,7 +50,8 @@ def combine_name_and_namespace(name_and_namespace): def find_node_and_namespace(node, full_node_name): node_names_and_namespaces = node.get_node_names_and_namespaces() return first_match( - node_names_and_namespaces, lambda n: combine_name_and_namespace(n) == full_node_name + node_names_and_namespaces, + lambda n: combine_name_and_namespace(n) == full_node_name, ) @@ -190,20 +191,24 @@ def main(args=None): retry_delay = 3 # seconds for attempt in range(max_retries): try: - logger.debug("Waiting for the spawner lock to be acquired!") + logger.debug( + bcolors.OKGREEN + "Waiting for the spawner lock to be acquired!" + bcolors.ENDC + ) # timeout after 20 seconds and try again lock.acquire(timeout=20) - logger.debug("Spawner lock acquired!") + logger.debug(bcolors.OKGREEN + "Spawner lock acquired!" + bcolors.ENDC) break except Timeout: logger.warning( - "Attempt %d failed. Retrying in %d seconds...", - attempt + 1, - retry_delay, + bcolors.WARNING + + f"Attempt {attempt+1} failed. Retrying in {retry_delay} seconds..." + + bcolors.ENDC ) time.sleep(retry_delay) else: - logger.error("Failed to acquire lock after multiple attempts.") + logger.error( + bcolors.FAIL + "Failed to acquire lock after multiple attempts." + bcolors.ENDC + ) return 1 node = Node(spawner_node_name) @@ -229,7 +234,11 @@ def main(args=None): controller_manager_timeout, service_call_timeout, ): - logger.warning("Controller already loaded, skipping load_controller") + logger.warning( + bcolors.WARNING + + "Controller already loaded, skipping load_controller" + + bcolors.ENDC + ) else: if controller_ros_args := args.controller_ros_args: if not set_controller_parameters( @@ -258,9 +267,17 @@ def main(args=None): service_call_timeout, ) if not ret.ok: - logger.fatal("Failed loading controller %s", controller_name) + logger.fatal( + bcolors.FAIL + + "Failed loading controller " + + bcolors.BOLD + + controller_name + + bcolors.ENDC + ) return 1 - logger.info("Loaded %s", controller_name) + logger.info( + bcolors.OKBLUE + "Loaded " + bcolors.BOLD + controller_name + bcolors.ENDC + ) if not args.load_only: ret = configure_controller( @@ -271,7 +288,7 @@ def main(args=None): service_call_timeout, ) if not ret.ok: - logger.error("Failed to configure controller") + logger.error(bcolors.FAIL + "Failed to configure controller" + bcolors.ENDC) return 1 if not args.inactive and not args.activate_as_group: @@ -286,10 +303,18 @@ def main(args=None): service_call_timeout, ) if not ret.ok: - logger.error("Failed to activate controller : %s", controller_name) + logger.error( + f"{bcolors.FAIL}Failed to activate controller : {controller_name}{bcolors.ENDC}" + ) return 1 - logger.info("Configured and activated %s", controller_name) + logger.info( + bcolors.OKGREEN + + "Configured and activated " + + bcolors.BOLD + + controller_name + + bcolors.ENDC + ) if not args.inactive and args.activate_as_group: ret = switch_controllers( @@ -304,12 +329,14 @@ def main(args=None): ) if not ret.ok: logger.error( - "Failed to activate the parsed controllers list : %s", controller_names + f"{bcolors.FAIL}Failed to activate the parsed controllers list : {controller_names}{bcolors.ENDC}" ) return 1 logger.info( - "Configured and activated all the parsed controllers list : %s!", controller_names + bcolors.OKGREEN + + f"Configured and activated all the parsed controllers list : {controller_names}!" + + bcolors.ENDC ) unload_controllers_upon_exit = args.unload_on_kill if not unload_controllers_upon_exit: @@ -337,7 +364,7 @@ def main(args=None): service_call_timeout, ) if not ret.ok: - logger.error("Failed to deactivate controller") + logger.error(bcolors.FAIL + "Failed to deactivate controller" + bcolors.ENDC) return 1 logger.info(f"Successfully deactivated controllers : {controller_names}") @@ -353,7 +380,11 @@ def main(args=None): ) if not ret.ok: unload_status = False - logger.error("Failed to unload controller : %s", controller_name) + logger.error( + bcolors.FAIL + + f"Failed to unload controller : {controller_name}" + + bcolors.ENDC + ) if unload_status: logger.info(f"Successfully unloaded controllers : {controller_names}") From 694c4868e4182df76547ca50cea29f4a4dc8c591 Mon Sep 17 00:00:00 2001 From: sauman raaj <83863464+saumanraaj@users.noreply.github.com> Date: Sun, 2 Nov 2025 17:12:11 -0500 Subject: [PATCH 3/4] Add documentation for environment-aware color output - Document color output handling in userdoc.rst - Add release note about bcolors respecting RCUTILS_COLORIZED_OUTPUT --- controller_manager/doc/userdoc.rst | 10 ++++++++++ doc/release_notes.rst | 4 +++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index e96e767948..33a35f9332 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -493,3 +493,13 @@ The ``time`` argument in the ``read`` and ``write`` methods of the hardware comp The ``period`` argument in the ``read``, ``update`` and ``write`` methods is calculated using the trigger clock of type ``RCL_STEADY_TIME`` so it is always monotonic. The reason behind using different clocks is to avoid the issues related to the affect of system time changes in the realtime loops. The ``ros2_control_node`` now also detects the overruns caused by the system time changes and longer execution times of the controllers and hardware components. The controller manager will print a warning message if the controller or hardware component misses the update cycle due to the system time changes or longer execution times. + +Color Output Handling +^^^^^^^^^^^^^^^^^^^^^ + +The helper scripts (``spawner`` and ``hardware_spawner``) now use an environment-aware ``bcolors`` class. +The color output automatically adapts to the environment: + +* ``RCUTILS_COLORIZED_OUTPUT=0`` → disables color output +* ``RCUTILS_COLORIZED_OUTPUT=1`` → forces color output +* Unset → automatically detects TTY and enables color only in interactive terminals diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 903280545f..a759526204 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -10,7 +10,9 @@ controller_interface * The new ``MagneticFieldSensor`` semantic component provides an interface for reading data from magnetometers. `(#2627 `__) controller_manager -****************** +******************** +* The ``bcolors`` class now respects the ``RCUTILS_COLORIZED_OUTPUT`` environment variable + to automatically disable colors in non-TTY and CI environments. (`#2741 `__) hardware_interface ****************** From 689837a2d6fa88a27e29c0b9ea7e8387e12bf4e8 Mon Sep 17 00:00:00 2001 From: sauman raaj <83863464+saumanraaj@users.noreply.github.com> Date: Mon, 3 Nov 2025 07:51:54 -0500 Subject: [PATCH 4/4] docs: fix RST title underline, spacing, and ASCII arrows for doc8 --- controller_manager/doc/userdoc.rst | 7 ++++--- doc/release_notes.rst | 8 +++++--- 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 2097f5aa58..a2aaf766d9 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -501,6 +501,7 @@ Color Output Handling The helper scripts (``spawner`` and ``hardware_spawner``) now use an environment-aware ``bcolors`` class. The color output automatically adapts to the environment: -* ``RCUTILS_COLORIZED_OUTPUT=0`` → disables color output -* ``RCUTILS_COLORIZED_OUTPUT=1`` → forces color output -* Unset → automatically detects TTY and enables color only in interactive terminals +* ``RCUTILS_COLORIZED_OUTPUT=0`` -> disables color output +* ``RCUTILS_COLORIZED_OUTPUT=1`` -> forces color output +* Unset -> automatically detects TTY and enables color only in interactive + terminals diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 947604ef45..fd450cd04e 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -11,10 +11,9 @@ controller_interface * The controller_manager will now deactivate the entire controller chain if a controller in the chain fails during the update cycle. `(#2681 `__) controller_manager -******************** -* The ``bcolors`` class now respects the ``RCUTILS_COLORIZED_OUTPUT`` environment variable - to automatically disable colors in non-TTY and CI environments. (`#2741 `__) ****************** +* The ``bcolors`` class now respects the ``RCUTILS_COLORIZED_OUTPUT`` environment + variable to automatically disable colors in non-TTY and CI environments. * The default strictness for ``switch_controller`` is changed to ``strict``. (`#2742 `__) @@ -23,6 +22,9 @@ hardware_interface ros2controlcli ************** + +No notable changes in this release. + transmission_interface ********************** * The ``simple_transmission`` and ``differential_transmission`` now also support the ``force`` interface (`#2588 `_).