Ros2cli diagnostics tool

.github/workflows/lint.yaml

@@ -36,4 +36,5 @@ jobs:
             diagnostic_aggregator
             diagnostic_common_diagnostics
             diagnostic_updater
+            ros2diagnostics
             self_test

.github/workflows/test.yaml

@@ -19,6 +19,7 @@ jobs:
             diagnostic_aggregator,
             diagnostic_common_diagnostics,
             diagnostic_updater,
+            ros2diagnostics,
             self_test,
           ]
         distro: [humble, iron, rolling]

ros2diagnostics/README.md

@@ -0,0 +1,136 @@
+# ROS2 diagnostics 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 three verbs:
+
+- list
+- echo
+- 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
+```
+
+### echo
+Monitor `/diagnostics` topic and print the diagnostics_status data can filter by level and node/status name
+
+#### demo
+
+```bash title="show all diagnostics status"
+ros2 diagnostics echo
+# 
+--- 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 echo -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 echo -f Task2
+#
+--- time: 1682528688 ---
+diagnostic_simple: DemoTask2: ERROR, bad
+--- time: 1682528689 ---
+diagnostic_simple: DemoTask2: ERROR, bad
+```
+
+```bash title="verbose usage"
+ros2 diagnostics echo -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
+
+
+## Tests
+```
+ros2 launch diagnostic_aggregator example.launch.py
+```

ros2diagnostics/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="2">
+  <name>ros2diagnostics</name>
+  <version>3.1.2</version>
+  <description>diagnostic command for ROS2 command line, parse and echo diagnostics topic</description>
+  <maintainer email="[email protected]"></maintainer>
+  <maintainer email="[email protected]"></maintainer>
+  <maintainer email="[email protected]">Christian Henkel</maintainer>
+  <license>BSD-3-Clause</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>ros2cli</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
+  <exec_depend>osrf_pycommon</exec_depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <exec_depend>diagnostic_msgs</exec_depend>
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

ros2diagnostics/ros2diagnostics/api/__init__.py

@@ -0,0 +1,136 @@
+# Copyright 2023 robobe
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#
+#    * Neither the name of the robobe nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#      this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+from argparse import ArgumentParser
+from pathlib import Path
+from typing import Dict, List, TextIO, Tuple
+
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
+
+import rclpy
+from rclpy.qos import qos_profile_system_default
+from osrf_pycommon.terminal_color import format_color
+
+
+TOPIC_DIAGNOSTICS = '/diagnostics'
+
+level_to_str_mapping = {
+    DiagnosticStatus.OK: ('OK', format_color("@{greenf}OK@{reset}.")),
+    DiagnosticStatus.WARN: ('WARN', format_color("@{yellowf}WARN@{reset}.")),
+    DiagnosticStatus.ERROR: ('ERROR', format_color("@{redf}ERROR@{reset}.")),
+    DiagnosticStatus.STALE: ('STALE', format_color("@{redf}STALE@{reset}.")),
+}
+
+
+def convert_level_to_str(level: bytes) -> Tuple[str, str]:
+    return level_to_str_mapping[level]
+
+
+def open_file_for_output(csv_file: str) -> TextIO:
+    csv_path = Path(csv_file)
+    if not csv_path.parent.is_dir():
+        raise FileNotFoundError(
+            'Cannot write file, directory does not exist'
+        )
+    return csv_path.open('w', encoding='utf-8')
+
+
+def map_level_from_name(levels: List[str]) -> List[bytes]:
+    b_levels = []
+    if levels is None:
+        return b_levels
+
+    map_levels = {
+        'info': lambda: b_levels.append(b'\x00'),
+        'warn': lambda: b_levels.append(b'\x01'),
+        'error': lambda: b_levels.append(b'\x02'),
+    }
+
+    for level in levels:
+        map_levels[level]()
+    return b_levels
+
+
+class DiagnosticsParser:
+    def __init__(
+        self,
+        verbose=False,
+        levels=None,
+        run_once=False,
+    ) -> None:
+        self.__run_once = run_once
+        self.__verbose = verbose
+        self.__levels_info = ','.join(levels) if levels is not None else ''
+        self.__levels = map_level_from_name(levels)
+
+    def filter_level(self, level):
+        return False if not self.__levels else level not in self.__levels
+
+    def register_diagnostics_topic(self, handler):
+        """Create ros node and subscribe to /diagnostic topic."""
+
+        rclpy.init()
+        node = rclpy.create_node('ros2diagnostics_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 add_common_arguments(parser: ArgumentParser):
+    """Add common arguments for csv and echo verbs."""
+    parser.add_argument(
+        '-1',
+        '--once',
+        action='store_true',
+        help='run only once'
+    )
+    parser.add_argument(
+        '-f',
+        '--filter',
+        type=str,
+        help='filter by 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/ros2diagnostics/command/diagnostics.py

@@ -0,0 +1,53 @@
+# Copyright 2023 robobe
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#
+#    * Neither the name of the robobe nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#      this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+
+from ros2cli.command import add_subparsers_on_demand
+from ros2cli.command import CommandExtension
+
+
+class DiagnosticsCommand(CommandExtension):
+    """Various diagnostics related sub-commands."""
+
+    def add_arguments(self, parser, cli_name):
+        self._subparser = parser
+        add_subparsers_on_demand(
+            parser,
+            cli_name,
+            '_verb',
+            'ros2diagnostics.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)