From 8252a85584ee5f6b2d6073516cb1361d2e543cab Mon Sep 17 00:00:00 2001
From: bijoua29 <73511637+bijoua29@users.noreply.github.com>
Date: Sun, 19 Mar 2023 15:02:02 -0700
Subject: [PATCH] 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/spawner.py             | 40 ++++++++++++++++---
 1 file changed, 34 insertions(+), 6 deletions(-)

diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py
index e33ab19395..d14dc538e6 100644
--- a/controller_manager/controller_manager/spawner.py
+++ b/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 "'