From 6a607341369e67d240ff7d6efa9b9aff98553014 Mon Sep 17 00:00:00 2001 From: Alexander Tiderko Date: Wed, 20 Dec 2023 11:02:30 +0100 Subject: [PATCH] fixed parsing of diagnostic messages --- .../fkie_node_manager_daemon/monitor/service.py | 7 +++++-- .../fkie_node_manager_daemon/monitor_servicer.py | 16 +++++++--------- .../fkie_node_manager_daemon/ros_node.py | 5 +++-- .../fkie_node_manager_daemon/server.py | 11 +++++++++-- 4 files changed, 24 insertions(+), 15 deletions(-) diff --git a/fkie_node_manager_daemon/fkie_node_manager_daemon/monitor/service.py b/fkie_node_manager_daemon/fkie_node_manager_daemon/monitor/service.py index 2a8a5cc9..da6d2058 100644 --- a/fkie_node_manager_daemon/fkie_node_manager_daemon/monitor/service.py +++ b/fkie_node_manager_daemon/fkie_node_manager_daemon/monitor/service.py @@ -72,11 +72,12 @@ def __gt__(self, item): class Service: - def __init__(self, settings: Settings): + def __init__(self, settings: Settings, callbackDiagnostics=None): self._clock = Clock() self._settings = settings self._mutex = threading.RLock() self._diagnostics = [] # DiagnosticObj + self._callbackDiagnostics = callbackDiagnostics self.use_diagnostics_agg = settings.param( 'global/use_diagnostics_agg', True) self._sub_diag = None @@ -115,7 +116,7 @@ def reload_parameter(self, settings: Settings): def _callback_diagnostics(self, msg: DiagnosticArray): # TODO: update diagnostics with self._mutex: - stamp = msg.header.stamp.to_sec() + stamp = float(msg.header.stamp.sec) + float(msg.header.stamp.nanosec) / 1000000000.0 for status in msg.status: try: idx = self._diagnostics.index(status) @@ -126,6 +127,8 @@ def _callback_diagnostics(self, msg: DiagnosticArray): diag_obj = DiagnosticObj(status, stamp) diag_obj.timestamp = stamp self._diagnostics.append(diag_obj) + if self._callbackDiagnostics: + self._callbackDiagnostics(msg) def get_system_diagnostics(self, filter_level: list = [], filter_ts: float = 0): result = DiagnosticArray() diff --git a/fkie_node_manager_daemon/fkie_node_manager_daemon/monitor_servicer.py b/fkie_node_manager_daemon/fkie_node_manager_daemon/monitor_servicer.py index 23aec3e0..dc696b13 100644 --- a/fkie_node_manager_daemon/fkie_node_manager_daemon/monitor_servicer.py +++ b/fkie_node_manager_daemon/fkie_node_manager_daemon/monitor_servicer.py @@ -22,7 +22,6 @@ import json from fkie_node_manager_daemon.monitor import Service -from fkie_multimaster_pylib.settings import Settings from fkie_multimaster_pylib.crossbar.runtime_interface import DiagnosticArray from fkie_multimaster_pylib.crossbar.runtime_interface import DiagnosticStatus @@ -31,9 +30,7 @@ from fkie_multimaster_pylib.crossbar.base_session import CrossbarBaseSession from fkie_multimaster_pylib.crossbar.base_session import SelfEncoder from fkie_multimaster_pylib.logging.logging import Log -import fkie_node_manager_daemon as nmd -from . import version class MonitorServicer(CrossbarBaseSession): def __init__( @@ -41,10 +38,11 @@ def __init__( ): Log.info("Create monitor servicer") CrossbarBaseSession.__init__(self, loop, realm, port) - self._version, self._date = version.detect_version( - nmd.ros_node, "fkie_node_manager_daemon" - ) - self._monitor = Service(Settings(version=self._version), self.diagnosticsCbPublisher) + self._monitor = Service(settings, self.diagnosticsCbPublisher) + + def stop(self): + self._monitor.stop() + self.shutdown() @wamp.register("ros.provider.get_system_info") def getSystemInfo(self) -> SystemInformation: @@ -58,8 +56,8 @@ def getSystemEnv(self) -> SystemEnvironment: def _toCrossbarDiagnostics(self, rosmsg): cbMsg = DiagnosticArray( - float(rosmsg.header.stamp.secs) - + float(rosmsg.header.stamp.nsecs) / 1000000000.0, [] + timestamp=float(rosmsg.header.stamp.sec) + + float(rosmsg.header.stamp.nanosec) / 1000000000.0, status=[] ) for sensor in rosmsg.status: values = [] diff --git a/fkie_node_manager_daemon/fkie_node_manager_daemon/ros_node.py b/fkie_node_manager_daemon/fkie_node_manager_daemon/ros_node.py index 2d1e1190..7e3af3b6 100644 --- a/fkie_node_manager_daemon/fkie_node_manager_daemon/ros_node.py +++ b/fkie_node_manager_daemon/fkie_node_manager_daemon/ros_node.py @@ -112,8 +112,9 @@ def spin(self): try: rclpy.shutdown() except Exception: - import traceback - print(traceback.format_exc()) + pass + # import traceback + # print(traceback.format_exc()) print('bye!') def _run_tests(self): diff --git a/fkie_node_manager_daemon/fkie_node_manager_daemon/server.py b/fkie_node_manager_daemon/fkie_node_manager_daemon/server.py index a63e4cac..96389cb2 100644 --- a/fkie_node_manager_daemon/fkie_node_manager_daemon/server.py +++ b/fkie_node_manager_daemon/fkie_node_manager_daemon/server.py @@ -41,6 +41,7 @@ from fkie_multimaster_pylib.launch import xml from fkie_multimaster_pylib.names import ns_join from fkie_multimaster_pylib.crossbar import server +from fkie_multimaster_pylib.settings import Settings from fkie_multimaster_pylib.system.host import get_host_name from fkie_multimaster_pylib.crossbar.base_session import SelfEncoder from fkie_multimaster_pylib.logging.logging import Log @@ -53,6 +54,7 @@ from fkie_node_manager_daemon.rosstate_servicer import RosStateServicer from fkie_node_manager_daemon.screen_servicer import ScreenServicer from fkie_node_manager_daemon.version_servicer import VersionServicer +from fkie_node_manager_daemon.version import detect_version class Server: @@ -61,6 +63,10 @@ def __init__(self, rosnode, *, default_domain_id=-1): self.crossbar_port = server.port() self.crossbar_realm = "ros" self.crossbar_loop = asyncio.get_event_loop() + self._version, self._date = detect_version( + nmd.ros_node, "fkie_node_manager_daemon" + ) + self._settings = Settings(version=self._version) self.ros_domain_id = default_domain_id if self.ros_domain_id > 0: rosnode.get_logger().warn( @@ -69,7 +75,7 @@ def __init__(self, rosnode, *, default_domain_id=-1): self.name = get_host_name() self.uri = "" self.monitor_servicer = MonitorServicer( - self.crossbar_loop, self.crossbar_realm, self.crossbar_port + self._settings, self.crossbar_loop, self.crossbar_realm, self.crossbar_port ) self.file_servicer = FileServicer( self.crossbar_loop, self.crossbar_realm, self.crossbar_port @@ -163,6 +169,7 @@ def _crossbar_notify_if_regsitered(self): registration_finished &= self.rosstate_servicer.crossbar_registered registration_finished &= self.parameter_servicer.crossbar_registered registration_finished &= self.version_servicer.crossbar_registered + registration_finished &= self.monitor_servicer.crossbar_registered time.sleep(0.5) self.publish_daemon_state(True) self._crossbar_send_status(True) @@ -190,7 +197,7 @@ def shutdown(self): self.screen_servicer.stop() self.launch_servicer.stop() self.file_servicer.stop() - # self.monitor_servicer.stop() + self.monitor_servicer.stop() self.rosstate_servicer.stop() self.screen_servicer.stop() self.parameter_servicer.stop()