[WIP] Merge develop/pr2 branch

doc/jsk_tools/scripts/audible_warning.md

@@ -0,0 +1,156 @@
+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 / 100.0`)
+
+    Rate of speak loop. If `~wait_speak` is `True`, wait until a robot finish speaking.
+
+* `~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.
+
+* `~wait_speak_duration_time` (`Float`, default: `30.0`)
+
+    Waiting time in `robotsound` action.
+
+* `~enable` (`Bool`, default: `True`)
+
+    If `True`, speak diagnositcs. If `False`, this node don't speak.
+
+* `~speak_ok` (`Bool`, default: `False`)
+
+    If `True`, speak ok level diagnostics.
+
+* `~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"
+    ```
+
+* `~ignore_time_after_runstop_is_enabled` (`Float`, default: `0.0`)
+
+    Time to ignore diagnostics after runstop is enabled.
+
+* `~ignore_time_after_runstop_is_disabled` (`Float`, default: `0.0`)
+
+    Time to ignore diagnostics after runstop is disabled.
+
+- `~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

@@ -1,11 +1,18 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(jsk_tools)
 
-find_package(catkin REQUIRED)
+find_package(catkin REQUIRED
+  COMPONENTS
+  dynamic_reconfigure)
 catkin_python_setup()
 set(ROS_BUILD_TYPE RelWithDebInfo)
+
+generate_dynamic_reconfigure_options(
+  cfg/AudibleWarning.cfg
+)
+
 catkin_package(
-  CATKIN_DEPENDS #
+  CATKIN_DEPENDS dynamic_reconfigure #
   LIBRARIES #
   INCLUDE_DIRS #
   DEPENDS #
@@ -27,6 +34,7 @@ endforeach(exec)
 
 if (CATKIN_ENABLE_TESTING)
   find_package(roslint REQUIRED)
+  roslint_python(node_scripts/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 +57,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)
   jsk_tools_add_rostest(test/test_sanity_diagnostics.test)
@@ -74,7 +83,7 @@ install(DIRECTORY test
   USE_SOURCE_PERMISSIONS
   PATTERN "*.test" EXCLUDE
   )
-install(DIRECTORY src dot-files cmake launch sample
+install(DIRECTORY src dot-files cmake launch sample node_scripts
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
   USE_SOURCE_PERMISSIONS
   )

jsk_tools/cfg/AudibleWarning.cfg

@@ -0,0 +1,26 @@
+#! /usr/bin/env python
+
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+PACKAGE = 'jsk_tools'
+
+gen = ParameterGenerator()
+speak_group = gen.add_group("speak flag")
+speak_group.add("enable", bool_t, 0, "Flag of speak", True)
+speak_group.add("speak_ok", bool_t, 0, "Speak ok level diagnostics", False)
+speak_group.add("speak_stale", bool_t, 0, "Speak stale level diagnostics", True)
+speak_group.add("speak_warn", bool_t, 0, "Speak warn level diagnostics", True)
+speak_group.add("speak_error", bool_t, 0, "Speak error level diagnostics", True)
+speak_group.add("speak_when_runstopped", bool_t, 0, "Speak when runstop is enabled", True)
+
+gen.add("volume", double_t, 0, "Volume of sound.", 1.0, 0.0, 1.0)
+gen.add("speak_interval", double_t, 0, "Volume of sound.", 120.0, 0.0, 3600.0)
+gen.add("ignore_time_after_runstop_is_enabled", double_t, 0,
+        "Time to ignore diagnostics after runstop is enabled.",
+        0.0, 0.0, 3600.0)
+gen.add("ignore_time_after_runstop_is_disabled", double_t, 0,
+        "Time to ignore diagnostics after runstop is disabled.",
+        0.0, 0.0, 3600.0)
+
+exit(gen.generate(PACKAGE, PACKAGE, "AudibleWarning"))