Ros2cli diagnostics

ros2diagnostics_cli/README.md

@@ -0,0 +1,146 @@
+# ROS2 diagnostic cli
+
+ROS2 cli to analysis and monitor `/diagnostics` topic
+It's alternative to `diagnostic_analysis` project that not ported yet to ROS2.
+
+The project add `diagnostics` command to ROS2 cli with there verbs.
+
+- list
+- show
+- csv
+
+### list
+Monitor the current diagnostics_status message from `/diagnostics` topic group by Node name and print `diagnostics status` name
+
+```bash
+ros2 diagnostics list   
+# Result
+--- time: 1682528234 ---
+diagnostic_simple:
+- DemoTask
+- DemoTask2
+```
+
+### show
+Monitor `/diagnostics` topic and print the diagnostics_status data can filter by level and node/status name
+
+```bash
+ros2 diagnostics show -h
+usage: ros2 diagnostics show [-h] [-1] [-f FILTER] [--verbose] [-l {info,warn,error}]
+
+Show diagnostics status item info
+
+options:
+  -h, --help            show this help message and exit
+  -1, --once            run only once
+  -f FILTER, --filter FILTER
+                        filter diagnostic status name
+  --verbose, -v         Display more info.
+  -l {info,warn,error}, --levels {info,warn,error}
+                        levels to filter, can be multiple times
+```
+
+#### demo
+
+```bash title="show all diagnostics status"
+ros2 diagnostics show
+# 
+--- time: 1682528494 ---
+diagnostic_simple: DemoTask: WARN, running
+diagnostic_simple: DemoTask2: ERROR, bad
+--- time: 1682528495 ---
+diagnostic_simple: DemoTask: WARN, running
+diagnostic_simple: DemoTask2: ERROR, bad
+```
+
+```bash title="filter by level"
+ros2 diagnostics show -l error
+--- time: 1682528568 ---
+diagnostic_simple: DemoTask2: ERROR, bad
+--- time: 1682528569 ---
+diagnostic_simple: DemoTask2: ERROR, bad
+--- time: 1682528570 ---
+```
+
+```bash title="filter by name"
+ros2 diagnostics show -f Task2
+#
+--- time: 1682528688 ---
+diagnostic_simple: DemoTask2: ERROR, bad
+--- time: 1682528689 ---
+diagnostic_simple: DemoTask2: ERROR, bad
+```
+
+```bash title="verbose usage"
+ros2 diagnostics show -l warn -v
+#
+--- time: 1682528760 ---
+diagnostic_simple: DemoTask: WARN, running
+- key1=val1
+- key2=val2
+--- time: 1682528761 ---
+diagnostic_simple: DemoTask: WARN, running
+- key1=val1
+- key2=val2
+
+```
+
+### csv
+Export `/diagnostics` topic to csv file
+
+**CSV headers**:
+- time (sec)
+- level
+- node name
+- diagnostics status name
+- message
+- hardware id
+- values from keyvalue field (only on verbose)
+
+
+```bash
+ros2 diagnostics csv --help
+usage: ros2 diagnostics csv [-h] [-1] [-f FILTER] [-l {info,warn,error}] [--output OUTPUT] [--verbose]
+
+export /diagnostics message to csv file
+
+options:
+  -h, --help            show this help message and exit
+  -1, --once            run only once
+  -f FILTER, --filter FILTER
+                        filter diagnostic status name
+  -l {info,warn,error}, --levels {info,warn,error}
+                        levels to filter, can be multiple times
+  --output OUTPUT, -o OUTPUT
+                        export file full path
+  --verbose, -v         export DiagnosticStatus values filed
+```
+
+#### Demos
+
+```bash title="simple csv file"
+ros2 diagnostics csv -o /tmp/1.csv
+--- time: 1682529183 ---
+1682529183,WARN,diagnostic_simple,DemoTask,running,
+```
+
+```bash title="show csv file"
+cat /tmp/1.csv
+
+1682529183,WARN,diagnostic_simple,DemoTask,running,
+1682529183,ERROR,diagnostic_simple,DemoTask2,bad,
+```
+
+```bash title="filter by level"
+ ros2 diagnostics csv -o /tmp/1.csv -l error
+```
+
+```bash title="filter by name with regex"
+ros2 diagnostics csv -o /tmp/1.csv -f Task$ -v
+```
+
+## Todo
+- More tests
+- Add unit test
+- DEB package and install tests
+- Ideas

ros2diagnostics_cli/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ros2diagnostics_cli</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="robo2020@gmail.com">user</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+  <depend>ros2cli</depend>
+  <depend>python3-pyyaml</depend>
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

ros2diagnostics_cli/ros2diagnostics_cli/api/__init__.py

@@ -0,0 +1,191 @@
+"""_summary_
+
+std_msgs/Header header # for timestamp
+builtin_interfaces/Time stamp
+    int32 sec
+    uint32 nanosec
+string frame_id
+DiagnosticStatus[] status # an array of components being reported on
+byte OK=0
+byte WARN=1
+byte ERROR=2
+byte STALE=3
+byte level
+string name
+string message
+string hardware_id
+KeyValue[] values
+    string key
+    string value
+"""
+from typing import Dict, List, Tuple, TextIO
+import yaml
+import rclpy
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
+from rclpy.qos import qos_profile_system_default
+from argparse import ArgumentParser
+import pathlib
+import re
+from enum import IntEnum
+
+TOPIC_DIAGNOSTICS = "/diagnostics"
+
+COLOR_DEFAULT = "\033[39m"
+GREEN = "\033[92m"
+RED = "\033[91m"
+YELLOW = "\033[93m"
+
+
+def open_file_for_output(csv_file) -> TextIO:
+    base_dir = pathlib.Path(csv_file).parents[0]
+    folder_exists = pathlib.Path(base_dir).is_dir()
+    if not folder_exists:
+        raise Exception(f"Folder {csv_file} not exists")
+    f = open(csv_file, "w", encoding="utf-8")
+    return f
+
+class ParserModeEnum(IntEnum):
+    List = 1
+    CSV = 2
+    Show = 3
+class DiagnosticsParser:
+    def __init__(self, mode: ParserModeEnum, verbose=False, levels=None, run_once=False, name_filter=None) -> None:
+        self.__name_filter = name_filter
+        self.__status_render_handler = self.render
+        self.__mode = mode
+        self.__run_once = run_once
+        self.__verbose = verbose
+        self.__levels_info = ",".join(levels) if levels is not None else ""
+        self.__levels = DiagnosticsParser.map_level_from_name(levels)
+
+    def set_render(self, handler):
+        self.__status_render_handler = handler
+
+    def run(self):
+        self.register_and_parse_diagnostics_topic()
+
+    def __filter_level(self, level):
+        if not self.__levels:
+            return False
+        return level not in self.__levels
+
+    @staticmethod
+    def map_level_from_name(levels: List[str]) -> List[bytes]:
+        b_levels = []
+        if levels is None:
+            return b_levels
+
+        for level in levels:
+            match level:
+                case "info":
+                    b_levels.append(b"\x00")
+                case "warn":
+                    b_levels.append(b"\x01")
+                case "error":
+                    b_levels.append(b"\x02")
+        return b_levels
+
+
+
+    @staticmethod
+    def convert_level_to_str(level) -> Tuple[str, str]:
+        match level:
+            case b"\x00":
+                return ("OK", GREEN + "OK" + COLOR_DEFAULT)
+            case b"\x01":
+                return ("WARN", YELLOW + "WARN" + COLOR_DEFAULT)
+            case b"\x02":
+                return ("ERROR", RED + "ERROR" + COLOR_DEFAULT)
+            case b"\x03":
+                return ("STALE", RED + "STALE" + COLOR_DEFAULT)
+            case _:
+                return ("UNDEFINED", "UNDEFINED")
+
+    def render(self, status: DiagnosticStatus, time_sec, verbose):
+        _, level_name = DiagnosticsParser.convert_level_to_str(status.level)
+        item = f"{status.name}: {level_name}, {status.message}"
+        print(item)
+        if verbose:
+            kv: KeyValue
+            for kv in status.values:
+                print(f"- {kv.key}={kv.value}")
+
+    def diagnostics_status_handler(self, msg: DiagnosticArray) -> None:
+        """Run handler for each DiagnosticStatus in array
+        filter status by level, name, node name
+
+        Args:
+            msg (DiagnosticArray): _description_
+        """
+        counter: int = 0
+        status: DiagnosticStatus
+        print(f"--- time: {msg.header.stamp.sec} ---")
+        for status in msg.status:
+            if self.__name_filter:
+                result = re.search(self.__name_filter, status.name)
+                if not result:
+                    continue
+            if self.__filter_level(status.level):
+                continue
+            self.__status_render_handler(status, msg.header.stamp.sec, self.__verbose)
+            counter += 1
+
+        if not counter:
+            print(f"No diagnostic for levels: {self.__levels_info}")
+
+    def register_and_parse_diagnostics_topic(self):
+        match self.__mode:
+            case ParserModeEnum.List:
+                handler = diagnostic_list_handler
+            case _ :
+                handler = self.diagnostics_status_handler
+
+        rclpy.init()
+        node = rclpy.create_node("ros2diagnostics_cli_filter")
+        node.create_subscription(
+            DiagnosticArray,
+            TOPIC_DIAGNOSTICS,
+            handler,
+            qos_profile=qos_profile_system_default,
+        )
+        try:
+            if self.__run_once:
+                rclpy.spin_once(node)
+            else:
+                rclpy.spin(node)
+        finally:
+            node.destroy_node()
+            rclpy.try_shutdown()
+
+
+def diagnostic_list_handler(msg: DiagnosticArray) -> None:
+    """Group diagnostics Task by node
+    print group data as yaml to stdout
+
+    Args:
+        msg (DiagnosticArray): _description_
+    """
+    status: DiagnosticStatus
+    data: Dict[str, List[str]] = {}
+    print(f"--- time: {msg.header.stamp.sec} ---")
+    for status in msg.status:
+        node, name = status.name.split(":")
+        name = name.strip()
+        if node in data:
+            data[node].append(name)
+        else:
+            data[node] = [name]
+
+    print(yaml.dump(data))
+
+def add_common_arguments(parser: ArgumentParser):
+    parser.add_argument("-1", "--once", action="store_true", help="run only once")
+    parser.add_argument("-f", "--filter", type=str, help="filter diagnostic status name")
+    parser.add_argument(
+            "-l",
+            "--levels",
+            action="append",
+            type=str,
+            choices=["info", "warn", "error"],
+            help="levels to filter, can be multiple times",
+        )

ros2diagnostics_cli/ros2diagnostics_cli/command/diagnostics.py

@@ -0,0 +1,22 @@
+from ros2cli.command import add_subparsers_on_demand
+from ros2cli.command import CommandExtension
+
+
+class DiagCommand(CommandExtension):
+    def __init__(self):
+        super(DiagCommand, self).__init__()
+        self._subparser = None
+
+    def add_arguments(self, parser, cli_name):
+        self._subparser = parser
+        add_subparsers_on_demand(
+            parser, cli_name, '_verb', "ros2diagnostics_cli.verb", required=False)
+
+    def main(self, *, parser, args):
+        if not hasattr(args, '_verb'):
+            self._subparser.print_help()
+            return 0
+
+        extension = getattr(args, '_verb')
+
+        return extension.main(args=args)