Add audible warning node

doc/jsk_tools/scripts/audible_warning.md

@@ -0,0 +1,136 @@
+audible\_warning.py
+=======================
+
+## What is this
+
+This is a general node that subscribes to `/diagnostics_agg` to speak the error content.
+Robots using diagnostics can use this node.
+
+## Target Action
+
+* `/robotsound` (`sound_play/SoundRequestAction`)
+
+    Target action name.
+
+    If it is a different server name, please remap it like `<remap from="/robotsound" to="/sound_play" />`.
+
+## Subscribing Topics
+
+* `/diagnostics_agg` (`diagnostic_msgs/DiagnosticArray`)
+
+    Aggregated diagnostics.
+
+
+## Parameter
+
+* `~speak_rate` (`Float`, default: `1.0`)
+
+    Rate of speak loop.
+
+* `~speak_interval` (`Float`, default: `120.0`)
+
+    The same error will not be spoken until this number of seconds has passed.
+
+* `~volume` (`Float`, default: `1.0`)
+
+    Volume of speaking.
+
+* `~language` (`String`, default: `""`)
+
+    Language parameters for `arg2` in `sound_play/SoundRequestAction`.
+
+* `~wait_speak` (`Bool`, default: `True`)
+
+    If `True`, wait until finish saying one error.
+
+* `~seconds_to_start_speaking` (`Float`, default: `0.0`)
+
+    It is the time to wait for the node to speak after it has started.
+
+    This is useful for ignoring errors that occur when the robot starts.
+
+* `~speak_warn` (`Bool`, default: `True`)
+
+    If `True`, speak warning level diagnostics.
+
+* `~speak_error` (`Bool`, default: `True`)
+
+    If `True`, speak error level diagnostics.
+
+* `~speak_stale` (`Bool`, default: `True`)
+
+    If `True`, speak stale level diagnostics.
+
+* `~speak_when_runstopped` (`Bool`, default: `True`)
+
+    If `True`, speak an error even if runstop is `True`.
+
+* `~run_stop_topic` (`String`, default: `None`)
+
+    Subscribe this topic if this value is specified.
+
+* `~run_stop_condition` (`String`, default: `m.data == True`)
+
+    Returning bool value condition using the given Python expression.
+    The Python expression can access any of the Python builtins plus:
+    ``topic`` (the topic of the message), ``m`` (the message) and ``t`` (time of message).
+
+    For example, ``~run_stop_topic`` is ``robot_state (fetch_driver_msgs/RobotState)`` and if you want to check whether a runstop is a pressed, you can do the following.
+
+    ```bash
+    run_stop_condition: "m.runstopped is True"
+    ```
+
+- `~blacklist` (`Yaml`, required)
+
+    User must always specify `name`. You can specify `message` as an option.
+
+    These values are matched using python regular expressions.
+
+    It is something like below:
+
+    ```
+    <!-- speak warning -->
+    <node name="audible_warning"
+          pkg="jsk_tools" type="audible_warning.py"
+          output="screen" >
+      <remap from="/robotsound" to="/sound_play" />
+      <rosparam>
+        run_stop_topic: robot_state
+        run_stop_condition: "m.runstopped is True"
+        seconds_to_start_speaking: 30
+        blacklist:
+          - "/CPU/CPU Usage/my_machine CPU Usage"
+          - "/SoundPlay/sound_play: Node State"
+          - "/Other/jsk_joy_node: Joystick Driver Status"
+          - "/Other/Combined Gyro"
+          - "/Other/l515_head l515_head_realsense2_camera_color: Frequency Status"
+          - "/Other/l515_head l515_head_realsense2_camera_confidence: Frequency Status"
+          - "/Other/l515_head l515_head_realsense2_camera_depth: Frequency Status"
+          - "/Other/l515_head l515_head_realsense2_camera_infra: Frequency Status"
+          - "/Other/ukf_se: Filter diagnostic updater"
+          - "/Peripherals/PS3 Controller"
+          - "/Sensors/IMU/IMU 1 Gyro"
+          - "/Sensors/IMU/IMU 2 Gyro"
+        run_stop_blacklist:
+          - "\\w*_(mcb|breaker)"
+          - "/Motor Control Boards/.*"
+          - "/Breakers/.*"
+      </rosparam>
+    </node>
+    ```
+
+- `~run_stop_blacklist` (`Yaml`, optional)
+
+    This is valid when run_stop is `True`. Blacklist at run_stop.
+
+
+## Usage
+
+Listen warnings from diagnostics.
+
+```bash
+roslaunch jsk_tools sample_audible_warning.launch
+```
+
+![audible_warning](./images/audible_warning.jpg)

jsk_tools/CMakeLists.txt

@@ -27,6 +27,7 @@ endforeach(exec)
 
 if (CATKIN_ENABLE_TESTING)
   find_package(roslint REQUIRED)
+  roslint_python(src/audible_warning.py)
   roslint_python(src/jsk_tools/cltool.py)
   roslint_python(src/test_topic_published.py)
   roslint_python(src/test_rosparam_set.py)
@@ -49,6 +50,7 @@ if (CATKIN_ENABLE_TESTING)
     # https://github.com/jsk-ros-pkg/jsk_common/pull/1293#issuecomment-164158260
     jsk_tools_add_rostest(test/test_rosparam_set.test)
   endif()
+  jsk_tools_add_rostest(test/test_audible_warning.test)
   jsk_tools_add_rostest(test/test_stdout.test)
   jsk_tools_add_rostest(test/test_rostopic_host_sanity.test)
   find_package(jsk_tools REQUIRED)

jsk_tools/sample/config/diagnostics_analyzer.yaml

@@ -0,0 +1,33 @@
+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
+  cameras:
+    type: diagnostic_aggregator/AnalyzerGroup
+    path: Sensors
+    analyzers:
+      pseudo_diagnostics1:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: sensor1
+        find_and_remove_prefix: 'pseudo_diagnostics: '
+        num_items: 1
+      pseudo_diagnostics2:
+        type: diagnostic_aggregator/GenericAnalyzer
+        path: sensor2
+        find_and_remove_prefix: 'pseudo_diagnostics: '
+        num_items: 1

jsk_tools/sample/pseudo_diagnostics.py

@@ -0,0 +1,56 @@
+#!/usr/bin/env python
+
+import diagnostic_msgs
+import diagnostic_updater
+from distutils.version import LooseVersion as version
+import pkg_resources
+import rospy
+
+
+def change_diagnostics(stat):
+    current_state = True
+
+    def _change_diagnostics(stat):
+        if current_state:
+            stat.summary(diagnostic_msgs.msg.DiagnosticStatus.OK,
+                         "Sensor is OK")
+        else:
+            stat.summary(diagnostic_msgs.msg.DiagnosticStatus.ERROR,
+                         "Sensor is not OK.")
+        current_state = not current_state
+        stat.add("Current State", current_state)
+        return stat
+    return _change_diagnostics
+
+
+class PseudoDiagnostics(object):
+
+    def __init__(self, sensor_name, duration_time=5):
+        self.updater = diagnostic_updater.Updater()
+        self.updater.setHardwareID(sensor_name)
+        self.updater.add("pseudo_diagnostics", change_diagnostics)
+        kwargs = dict(
+            period=rospy.Duration(duration_time),
+            callback=self.update_diagnostics,
+            oneshot=False,
+        )
+        if version(pkg_resources.get_distribution('rospy').version) \
+                >= version('1.12.0'):
+            # on >=kinetic, it raises ROSTimeMovedBackwardsException
+            # when we use rosbag play --loop.
+            kwargs['reset'] = True
+        self.timer = rospy.Timer(**kwargs)
+
+    def update_diagnostics(self, event):
+        self.updater.update()
+
+
+if __name__ == '__main__':
+    rospy.init_node('pseudo_diagnostics')
+    pusedo_diagnotics = PseudoDiagnostics(
+        sensor_name='sensor1',
+        duration_time=5)
+    pusedo_diagnotics = PseudoDiagnostics(
+        sensor_name='sensor2',
+        duration_time=10)
+    rospy.spin()

jsk_tools/sample/sample_audible_warning.launch

@@ -0,0 +1,41 @@
+<launch>
+
+  <arg name="gui" default="true" />
+  <arg name="device" default="" />
+  <arg name="launch_sound_play" default="true" />
+
+  <node name="pseudo_diagnostics"
+        pkg="jsk_tools" type="pseudo_diagnostics.py"
+        output="screen">
+  </node>
+
+  <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>
+
+  <node name="sound_play"
+        pkg="sound_play" type="soundplay_node.py"
+        if="$(arg launch_sound_play)" >
+    <param name="device" value="$(arg device)" />
+    <param name="loop_rate" value="100" />
+  </node>
+
+  <node name="audible_warning_for_fetch"
+        pkg="jsk_tools" type="audible_warning.py"
+        output="screen" >
+    <remap from="/robotsound" to="/sound_play" />
+    <rosparam>
+      speak_interval: 0
+      seconds_to_start_speaking: 5
+    </rosparam>
+  </node>
+
+  <group if="$(arg gui)" >
+    <node name="robot_monitor"
+          pkg="rqt_robot_monitor" type="rqt_robot_monitor" >
+    </node>
+  </group>
+
+</launch>