Add log level support to spawner node (#972)

* Add log-level support for spawner node

* Fix check for printing stdout/stderr output from load_parameter_file

* Fix pre-commit errors

controller_manager/controller_manager/spawner.py

@@ -19,6 +19,8 @@
 import sys
 import time
 import warnings
+import io
+from contextlib import redirect_stdout, redirect_stderr
 
 from controller_manager import (
  configure_controller,
@@ -182,6 +184,13 @@ def main(args=None):
  default=10,
  type=int,
  )
+ parser.add_argument(
+ "--log-level",
+ help="Log level for spawner node",
+ required=False,
+ choices=["debug", "info", "warn", "error", "fatal"],
+ default="info",
+ )
 
  command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:]
  args = parser.parse_args(command_line_args)
@@ -191,6 +200,15 @@ def main(args=None):
  param_file = args.param_file
  controller_type = args.controller_type
  controller_manager_timeout = args.controller_manager_timeout
+ log_level = args.log_level
+
+ loglevel_to_severity = {
+ "debug": rclpy.logging.LoggingSeverity.DEBUG,
+ "info": rclpy.logging.LoggingSeverity.INFO,
+ "warn": rclpy.logging.LoggingSeverity.WARN,
+ "error": rclpy.logging.LoggingSeverity.ERROR,
+ "fatal": rclpy.logging.LoggingSeverity.FATAL,
+ }
 
  if param_file and not os.path.isfile(param_file):
  raise FileNotFoundError(errno.ENOENT, os.strerror(errno.ENOENT), param_file)
@@ -200,6 +218,8 @@ def main(args=None):
  prefixed_controller_name = controller_namespace + "/" + controller_name
 
  node = Node("spawner_" + controller_name)
+ rclpy.logging.set_logger_level("spawner_" + controller_name, loglevel_to_severity[log_level])
+
  if not controller_manager_name.startswith("/"):
  spawner_namespace = node.get_namespace()
  if spawner_namespace != "/":
@@ -264,12 +284,20 @@ def main(args=None):
  )
 
  if param_file:
- load_parameter_file(
- node=node,
- node_name=prefixed_controller_name,
- parameter_file=param_file,
- use_wildcard=True,
- )
+ # load_parameter_file writes to stdout/stderr. Here we capture that and use node logging instead
+ with redirect_stdout(io.StringIO()) as f_stdout, redirect_stderr(
+ io.StringIO()
+ ) as f_stderr:
+ load_parameter_file(
+ node=node,
+ node_name=prefixed_controller_name,
+ parameter_file=param_file,
+ use_wildcard=True,
+ )
+ if f_stdout.getvalue():
+ node.get_logger().info(bcolors.OKCYAN + f_stdout.getvalue() + bcolors.ENDC)
+ if f_stderr.getvalue():
+ node.get_logger().error(bcolors.FAIL + f_stderr.getvalue() + bcolors.ENDC)
  node.get_logger().info(
  bcolors.OKCYAN
  + 'Loaded parameters file "'