fixed parsing of diagnostic messages

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()

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,20 +30,19 @@
 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__(
         self, settings, loop: asyncio.AbstractEventLoop, realm: str = "ros", port: int = 11911
     ):
         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 = []

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):

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()