Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Ros2cli diagnostics tool #328

Draft
wants to merge 26 commits into
base: ros2
Choose a base branch
from
Draft
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
fix: suport python 3.8
remove math case
remove some of pylint warnings
robo authored and russkel committed Oct 7, 2023

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
commit e03df55e1e1b8e99a307a52b61c6d1aa30b10bed
55 changes: 26 additions & 29 deletions ros2diagnostics_cli/ros2diagnostics_cli/api/__init__.py
Original file line number Diff line number Diff line change
@@ -47,15 +47,16 @@
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 argparse import ArgumentParser
from enum import IntEnum
from typing import Dict, List, TextIO, Tuple

import rclpy
import yaml
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
from rclpy.qos import qos_profile_system_default

TOPIC_DIAGNOSTICS = "/diagnostics"

@@ -64,6 +65,16 @@
RED = "\033[91m"
YELLOW = "\033[93m"

level_to_str_mapping = {
b"\x00": lambda: ("OK", GREEN + "OK" + COLOR_DEFAULT),
b"\x01": lambda: ("WARN", YELLOW + "WARN" + COLOR_DEFAULT),
b"\x02": lambda: ("ERROR", RED + "ERROR" + COLOR_DEFAULT),
b"\x03": lambda: ("STALE", RED + "STALE" + COLOR_DEFAULT)
}

def convert_level_to_str(level) -> Tuple[str, str]:
return level_to_str_mapping[level]()


def open_file_for_output(csv_file) -> TextIO:
base_dir = pathlib.Path(csv_file).parents[0]
@@ -75,9 +86,9 @@ def open_file_for_output(csv_file) -> TextIO:


class ParserModeEnum(IntEnum):
List = 1
LIST = 1
CSV = 2
Show = 3
SHOW = 3


class DiagnosticsParser:
@@ -90,7 +101,7 @@ def __init__(
name_filter=None,
) -> None:
self.__name_filter = name_filter
self.__status_render_handler = self.render
self.__status_render_handler = DiagnosticsParser.render
self.__mode = mode
self.__run_once = run_once
self.__verbose = verbose
@@ -125,21 +136,8 @@ def map_level_from_name(levels: List[str]) -> List[bytes]:
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)
def render(status: DiagnosticStatus, time_sec, verbose):
_, level_name = convert_level_to_str(status.level)
item = f"{status.name}: {level_name}, {status.message}"
print(item)
if verbose:
@@ -171,11 +169,10 @@ def diagnostics_status_handler(self, msg: DiagnosticArray) -> None:
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
if self.__mode == ParserModeEnum.LIST:
handler = diagnostic_list_handler
else:
handler = self.diagnostics_status_handler

rclpy.init()
node = rclpy.create_node("ros2diagnostics_cli_filter")
3 changes: 2 additions & 1 deletion ros2diagnostics_cli/ros2diagnostics_cli/verb/csv.py
Original file line number Diff line number Diff line change
@@ -34,6 +34,7 @@
open_file_for_output,
add_common_arguments,
ParserModeEnum,
convert_level_to_str
)
from diagnostic_msgs.msg import DiagnosticStatus, KeyValue

@@ -59,7 +60,7 @@ def add_arguments(self, parser, cli_name):
)

def render(self, status: DiagnosticStatus, time_sec, verbose=False):
level_name, _ = DiagnosticsParser.convert_level_to_str(status.level)
level_name, _ = convert_level_to_str(status.level)
node_name, name = status.name.split(":")
name = name.strip()
line = [
2 changes: 1 addition & 1 deletion ros2diagnostics_cli/ros2diagnostics_cli/verb/list.py
Original file line number Diff line number Diff line change
@@ -35,5 +35,5 @@ class ListVerb(VerbExtension):
"""List all diagnostic status items group by node name"""

def main(self, *, args):
diagnostic_parser = DiagnosticsParser(ParserModeEnum.List)
diagnostic_parser = DiagnosticsParser(ParserModeEnum.LIST)
diagnostic_parser.run()
2 changes: 1 addition & 1 deletion ros2diagnostics_cli/ros2diagnostics_cli/verb/show.py
Original file line number Diff line number Diff line change
@@ -49,7 +49,7 @@ def add_arguments(self, parser, cli_name):

def main(self, *, args):
diagnostic_parser = DiagnosticsParser(
mode=ParserModeEnum.Show,
mode=ParserModeEnum.SHOW,
verbose=args.verbose,
levels=args.levels,
run_once=args.once,