[jsk_tools] Add node to publish diagnostics based on topic and node status

doc/jsk_tools/index.rst

@@ -29,6 +29,14 @@ This package includes several useful tools and library for ROS software.
    ./python/*
 
 
+.. toctree::
+   :glob:
+   :maxdepth: 1
+   :caption: jsk_tools scripts
+
+   ./scripts/*
+
+
 .. toctree::
    :glob:
    :maxdepth: 1

doc/jsk_tools/scripts/sanity_diagnostics.md

@@ -0,0 +1,65 @@
+sanity\_diagnostics.py
+==============
+
+## What is this
+
+This node publishes essential robot topic and node status to `/diagnostics`.
+
+## Publishing Topics
+
+* `/diagnostics` (`diagnostic_msgs/DiagnosticStatus`)
+
+    Diagnostics topic
+
+## Subscribing Topics
+
+* topics specified in the `~sanity_targets` yaml file
+
+## Parameter
+
+* `~sanity_targets` (`String`, default: `/var/lib/robot/sanity_targets.yaml`)
+
+    Yaml file which contains topics and nodes to be monitored.
+
+    Sample yaml format for fetch:
+
+    ```yaml
+    topics:
+      - /audio
+      - /base_scan
+      - /battery_state
+      - /edgetpu_human_pose_estimator/output/image
+      - /edgetpu_object_detector/output/image
+      - /gripper/imu
+      - /head_camera/depth/image_raw
+      - /head_camera/rgb/image_raw
+      - /imu
+      - /insta360/image_raw
+      - /joint_states
+      - /tf
+
+    nodes:
+      - /amcl
+      - /auto_dock
+      - /gripper_driver
+      - /head_camera/head_camera_nodelet_manager
+      - /move_base
+      - /move_group
+      - /respeaker_node
+      - /robot_driver
+      - /roswww
+    ```
+
+* `~duration` (`Float`, default: `1`)
+
+    Duration in which sanity is checked and `/diagnostics` is published.
+
+## Usage
+
+Visualize topic and node diagnostics.
+
+```bash
+roslaunch jsk_tools sample_sanity_diagnostics.launch
+```
+
+![sanity_diagnostics](https://user-images.githubusercontent.com/19769486/168039418-0171a6dc-9507-436d-a9a6-13a2c7f52327.png)

jsk_tools/CMakeLists.txt

@@ -51,6 +51,7 @@ if (CATKIN_ENABLE_TESTING)
   endif()
   jsk_tools_add_rostest(test/test_stdout.test)
   jsk_tools_add_rostest(test/test_rostopic_host_sanity.test)
+  jsk_tools_add_rostest(test/test_sanity_diagnostics.test)
   find_package(jsk_tools REQUIRED)
   jsk_tools_add_shell_test(COMMAND echo "testing jsk_tools_add_shell_test")
 endif()
@@ -73,7 +74,7 @@ install(DIRECTORY test
   USE_SOURCE_PERMISSIONS
   PATTERN "*.test" EXCLUDE
   )
-install(DIRECTORY src dot-files cmake launch
+install(DIRECTORY src dot-files cmake launch sample
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   USE_SOURCE_PERMISSIONS
   )

jsk_tools/package.xml

@@ -18,6 +18,9 @@
   <build_depend>git</build_depend>
   <build_depend>rosgraph_msgs</build_depend>
   <exec_depend>cv_bridge</exec_depend>
+  <exec_depend>diagnostic_aggregator</exec_depend>
+  <exec_depend>diagnostic_msgs</exec_depend>
+  <exec_depend>diagnostic_updater</exec_depend>
   <exec_depend>python-percol</exec_depend>
   <exec_depend condition="$ROS_PYTHON_VERSION == 2">python-colorama</exec_depend>
   <exec_depend condition="$ROS_PYTHON_VERSION == 3">python3-colorama</exec_depend>

jsk_tools/sample/config/diagnostics_analyzer.yaml

@@ -0,0 +1,19 @@
+analyzers:
+  topics:
+    type: diagnostic_aggregator/AnalyzerGroup
+    path: Topics
+    analyzers:
+      head_camera:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: HeadCamera
+        contains: 'head_camera'
+        num_items: 1
+  nodes:
+    type: diagnostic_aggregator/AnalyzerGroup
+    path: Nodes
+    analyzers:
+      robot_driver:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: RobotDriver
+        contains: 'robot_driver'
+        num_items: 1

jsk_tools/sample/config/sanity_targets.yaml

@@ -0,0 +1,4 @@
+topics:
+  - /head_camera/rgb/image_raw
+nodes:
+  - /robot_driver

jsk_tools/sample/sample_sanity_diagnostics.launch

@@ -0,0 +1,42 @@
+<launch>
+  <arg name="gui" default="true" />
+
+  <!-- Publish target topic -->
+  <node name="image_publisher" pkg="rostopic" type="rostopic"
+        args="pub -s /head_camera/rgb/image_raw sensor_msgs/Image &quot;{
+              'header': {
+                'seq': 0,
+                'stamp': {'secs': 0, 'nsecs': 0},
+                'frame_id': ''
+                },
+              'height': 0,
+              'width': 0,
+              'encoding': '',
+              'is_bigendian': 0,
+              'step': 0,
+              'data': ''}&quot;" />
+
+  <!-- Launch target node -->
+  <node name="robot_driver" pkg="rostopic" type="rostopic"
+        args="pub -s /test std_msgs/Empty {}" />
+
+  <!-- Publish /diagnostics -->
+  <node name="sanity_diagnostics" pkg="jsk_tools" type="sanity_diagnostics.py">
+    <rosparam command="load" file="$(find jsk_tools)/sample/config/sanity_targets.yaml" />
+    <rosparam subst_value="true">
+      duration: 1
+    </rosparam>
+  </node>
+
+  <!-- Publish /diagnostics_agg -->
+  <node name="diagnostic_aggregator"
+        pkg="diagnostic_aggregator" type="aggregator_node"
+        clear_params="true" >
+    <rosparam command="load" file="$(find jsk_tools)/sample/config/diagnostics_analyzer.yaml" />
+  </node>
+
+  <!-- Visualize diagnostic aggregator -->
+  <node if="$(arg gui)"
+        name="rqt_robot_monitor" pkg="rqt_robot_monitor" type="rqt_robot_monitor" />
+
+</launch>

jsk_tools/src/jsk_tools/sanity_lib.py

@@ -8,6 +8,7 @@
 import subprocess
 from threading import Lock
 import sys
+from importlib import import_module
 import math
 try:
     import colorama
@@ -59,13 +60,27 @@ def __init__(self, topic_name, timeout=5, echo=False,
         self.deadline = rospy.Time.now() + rospy.Duration(timeout)
         self.echo = echo
         self.echo_noarr = echo_noarr
+        self.data_class = data_class
+
+        self.sub = rospy.Subscriber(
+            self.topic_name,
+            rospy.msg.AnyMsg,
+            callback=self.callback,
+            queue_size=1)
         print(' Checking %s for %d seconds' % (topic_name, timeout))
-        msg_class, _, _ = rostopic.get_topic_class(topic_name, blocking=True)
-        if (data_class is not None) and (msg_class is not data_class):
-            raise rospy.ROSException('Topic msg type is different.')
-        self.sub = rospy.Subscriber(topic_name, msg_class, self.callback)
 
     def callback(self, msg):
+        # Do not check topic type until the first topic comes in
+        if isinstance(msg, rospy.AnyMsg):
+            package, msg_type = msg._connection_header['type'].split('/')
+            ros_pkg = package + '.msg'
+            msg_class = getattr(import_module(ros_pkg), msg_type)
+            if (self.data_class is not None) and (msg_class is not self.data_class):
+                raise rospy.ROSException('Topic msg type is different.')
+            self.sub.unregister()
+            deserialized_sub = rospy.Subscriber(
+                self.topic_name, msg_class, self.callback)
+            self.sub = deserialized_sub
         if self.echo and self.msg is None:  # this is first time
             print(colored('--- Echo %s' % self.topic_name, 'purple'))
             field_filter = rostopic.create_field_filter(echo_nostr=False, echo_noarr=self.echo_noarr)
@@ -75,13 +90,16 @@ def callback(self, msg):
 
     def check(self):
         while not rospy.is_shutdown():
-            if self.msg is not None:
+            if self.msg is not None and not isinstance(self.msg, rospy.AnyMsg):
                 return True
             elif rospy.Time.now() > self.deadline:
                 return False
             else:
                 rospy.sleep(0.1)
 
+    def unregister(self):
+        self.sub.unregister()
+
 
 def checkTopicIsPublished(topic_name, class_name = None,
                           ok_message = "",
@@ -109,6 +127,7 @@ def checkTopicIsPublished(topic_name, class_name = None,
         if not checker.check():
             errorMessage(" %s is not published" % checker.topic_name)
             all_success = False
+        checker.unregister()
     if not all_success:
         if error_message:
             errorMessage(error_message)
@@ -199,19 +218,23 @@ def checkNodeState(target_node_name, needed, sub_success="", sub_fail=""):
             okMessage("Node " + target_node_name + " exists")
             if sub_success:
                 print(Fore.GREEN+"    "+sub_success+ Fore.RESET)
+            return True
         else:
             errorMessage("Node " + target_node_name + " exists unexpecetedly. This should be killed with rosnode kill")
             if sub_fail:
                 print(Fore.RED+"    "+sub_fail+ Fore.RESET)
+            return False
     else:
         if needed:
             errorMessage("Node " + target_node_name + " doesn't exists. This node is NEEDED")
             if sub_fail:
                 print(Fore.RED+"    "+sub_fail+ Fore.RESET)
+            return False
         else:
             okMessage("Node " + target_node_name + " doesn't exists")
             if sub_success:
                 print(Fore.GREEN+"    "+sub_success+ Fore.RESET)
+            return True
 
 def checkUSBExist(vendor_id, product_id, expect_usb_nums = 1, host="", success_msg = "", error_msg = ""):
     """check USB Exists

jsk_tools/src/sanity_diagnostics.py

@@ -0,0 +1,67 @@
+#!/usr/bin/env python
+
+# https://github.com/start-jsk/jsk_apc/blob/master/jsk_arc2017_baxter/node_scripts/sanity_check_for_pick.py # NOQA
+
+from diagnostic_msgs.msg import DiagnosticStatus
+import diagnostic_updater
+from jsk_tools.sanity_lib import checkNodeState, checkTopicIsPublished
+import rospy
+
+
+class SanityDiagnostics(object):
+    """
+    This node publishes essential robot topic and node status to /diagnostics.
+
+    The yaml file is like the following:
+    topics:
+      - /kinect_head/rgb/image_raw
+      - /tf
+    nodes:
+      - /kinect_head/kinect_head_nodelet_manager
+      - /respeaker_node
+    """
+    def __init__(self):
+        duration = rospy.get_param('~duration', 60)
+        # Get sanity target topic and node names
+        topics = rospy.get_param('~topics', [])
+        nodes = rospy.get_param('~nodes', [])
+        # Set diagnostic updater for each topic and node
+        self.updater = diagnostic_updater.Updater()
+        self.updater.setHardwareID("none")
+        for topic_name in topics:
+            self.updater.add(
+                topic_name,
+                lambda stat, tn=topic_name: self.check_topic(stat, tn))
+        for node_name in nodes:
+            self.updater.add(
+                node_name,
+                lambda stat, nn=node_name: self.check_node(stat, nn))
+        # Timer to call updater
+        self.timer = rospy.Timer(
+            rospy.Duration(duration), self.check_sanity)
+
+    def check_sanity(self, event):
+        self.updater.update()
+
+    def check_topic(self, stat, topic_name):
+        # Assume that topic is published at more than (1.0 / timeout) Hz
+        topic_state = checkTopicIsPublished(topic_name, timeout=10)
+        if topic_state:
+            stat.summary(DiagnosticStatus.OK, 'Topic is published')
+        else:
+            stat.summary(DiagnosticStatus.ERROR, 'Topic is not published')
+        return stat
+
+    def check_node(self, stat, node_name):
+        node_state = checkNodeState(node_name, needed=True)
+        if node_state:
+            stat.summary(DiagnosticStatus.OK, 'Node is alive')
+        else:
+            stat.summary(DiagnosticStatus.ERROR, 'Node is dead')
+        return stat
+
+
+if __name__ == '__main__':
+    rospy.init_node('sanity_diagnostics')
+    SanityDiagnostics()
+    rospy.spin()

jsk_tools/test/test_sanity_diagnostics.test

@@ -0,0 +1,16 @@
+<launch>
+  <include file="$(find jsk_tools)/sample/sample_sanity_diagnostics.launch">
+    <arg name="gui" value="false"/>
+  </include>
+
+  <test test-name="test_sanity_diagnostics" name="test_sanity_diagnostics"
+        pkg="jsk_tools" type="test_topic_published.py">
+    <rosparam>
+      topic_0: /diagnostics
+      timeout_0: 30
+      topic_1: /diagnostics_agg
+      timeout_1: 30
+    </rosparam>
+  </test>
+
+</launch>