From cce79ebb8c5b509b8bd97e1be26f0061a7227a70 Mon Sep 17 00:00:00 2001
From: "Dr. Denis" <denis@stoglrobotics.de>
Date: Wed, 14 Aug 2024 15:15:11 +0200
Subject: [PATCH] Make list controller and list hardware components immediately
 visualize the state. (#1606)

---
 .../controller_manager/spawner.py               |  2 +-
 .../ros2controlcli/verb/list_controllers.py     | 14 +++++++++++---
 .../verb/list_hardware_components.py            | 17 ++++++++++++++---
 3 files changed, 26 insertions(+), 7 deletions(-)

diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py
index bc8d218ea8..a8b5d25eff 100644
--- a/controller_manager/controller_manager/spawner.py
+++ b/controller_manager/controller_manager/spawner.py
@@ -47,7 +47,7 @@
 
 
 class bcolors:
-    HEADER = "\033[95m"
+    MAGENTA = "\033[95m"
     OKBLUE = "\033[94m"
     OKCYAN = "\033[96m"
     OKGREEN = "\033[92m"
diff --git a/ros2controlcli/ros2controlcli/verb/list_controllers.py b/ros2controlcli/ros2controlcli/verb/list_controllers.py
index 31ce814865..a26a1168a1 100644
--- a/ros2controlcli/ros2controlcli/verb/list_controllers.py
+++ b/ros2controlcli/ros2controlcli/verb/list_controllers.py
@@ -22,7 +22,7 @@
 from ros2controlcli.api import add_controller_mgr_parsers
 
 
-def print_controller_state(c, args):
+def print_controller_state(c, args, col_width_name, col_width_state, col_width_type):
     state_color = ""
     if c.state == "active":
         state_color = bcolors.OKGREEN
@@ -31,7 +31,9 @@ def print_controller_state(c, args):
     elif c.state == "unconfigured":
         state_color = bcolors.WARNING
 
-    print(f"{c.name:20s}[{c.type:20s}] {state_color}{c.state:10s}{bcolors.ENDC}")
+    print(
+        f"{state_color}{c.name:<{col_width_name}}{bcolors.ENDC} {c.type:<{col_width_type}}  {state_color}{c.state:<{col_width_state}}{bcolors.ENDC}"
+    )
     if args.claimed_interfaces or args.verbose:
         print("\tclaimed interfaces:")
         for claimed_interface in c.claimed_interfaces:
@@ -96,7 +98,13 @@ def add_arguments(self, parser, cli_name):
     def main(self, *, args):
         with NodeStrategy(args) as node:
             response = list_controllers(node, args.controller_manager)
+
+            # Structure data as table for nicer output
+            col_width_name = max(len(ctrl.name) for ctrl in response.controller)
+            col_width_type = max(len(ctrl.type) for ctrl in response.controller)
+            col_width_state = max(len(ctrl.state) for ctrl in response.controller)
+
             for c in response.controller:
-                print_controller_state(c, args)
+                print_controller_state(c, args, col_width_name, col_width_state, col_width_type)
 
             return 0
diff --git a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py
index 8a5884f2cb..e2b427b233 100644
--- a/ros2controlcli/ros2controlcli/verb/list_hardware_components.py
+++ b/ros2controlcli/ros2controlcli/verb/list_hardware_components.py
@@ -15,6 +15,8 @@
 from controller_manager import list_hardware_components
 from controller_manager.spawner import bcolors
 
+from lifecycle_msgs.msg import State
+
 from ros2cli.node.direct import add_arguments
 from ros2cli.node.strategy import NodeStrategy
 from ros2cli.verb import VerbExtension
@@ -39,17 +41,26 @@ def main(self, *, args):
             hardware_components = list_hardware_components(node, args.controller_manager)
 
             for idx, component in enumerate(hardware_components.component):
+                # Set activity color for nicer visualization
+                activity_color = bcolors.FAIL
+                if component.state.id == State.PRIMARY_STATE_UNCONFIGURED:
+                    activity_color = bcolors.WARNING
+                if component.state.id == State.PRIMARY_STATE_INACTIVE:
+                    activity_color = bcolors.MAGENTA
+                if component.state.id == State.PRIMARY_STATE_ACTIVE:
+                    activity_color = bcolors.OKGREEN
+
                 print(
-                    f"Hardware Component {idx+1}\n\tname: {component.name}\n\ttype: {component.type}"
+                    f"Hardware Component {idx+1}\n\tname: {activity_color}{component.name}{bcolors.ENDC}\n\ttype: {component.type}"
                 )
                 if hasattr(component, "plugin_name"):
-                    plugin_name = component.plugin_name
+                    plugin_name = f"{component.plugin_name}"
                 else:
                     plugin_name = f"{bcolors.WARNING}plugin name missing!{bcolors.ENDC}"
 
                 print(
                     f"\tplugin name: {plugin_name}\n"
-                    f"\tstate: id={component.state.id} label={component.state.label}\n"
+                    f"\tstate: id={component.state.id} label={activity_color}{component.state.label}{bcolors.ENDC}\n"
                     f"\tcommand interfaces"
                 )
                 for cmd_interface in component.command_interfaces: