[fetch15] Enable dynamic reconfigure for audible warning to stop it.

doc/jsk_tools/scripts/audible_warning.md

@@ -23,9 +23,9 @@ Robots using diagnostics can use this node.
 
 ## Parameter
 
-* `~speak_rate` (`Float`, default: `1.0`)
+* `~speak_rate` (`Float`, default: `1.0 / 100.0`)
 
-    Rate of speak loop.
+    Rate of speak loop. If `~wait_speak` is `True`, wait until a robot finish speaking.
 
 * `~speak_interval` (`Float`, default: `120.0`)
 
@@ -49,6 +49,18 @@ Robots using diagnostics can use this node.
 
     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.
@@ -81,6 +93,14 @@ Robots using diagnostics can use this node.
     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.

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,7 +34,7 @@ endforeach(exec)
 
 if (CATKIN_ENABLE_TESTING)
   find_package(roslint REQUIRED)
-  roslint_python(src/audible_warning.py)
+  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)
@@ -76,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"))

jsk_tools/src/audible_warning.py → jsk_tools/node_scripts/audible_warning.py

@@ -12,13 +12,19 @@
 import actionlib
 from diagnostic_msgs.msg import DiagnosticArray
 from diagnostic_msgs.msg import DiagnosticStatus
+from dynamic_reconfigure.server import Server
+import std_msgs.msg
 import rospy
 from sound_play.msg import SoundRequest
 from sound_play.msg import SoundRequestAction
 from sound_play.msg import SoundRequestGoal
 
+from jsk_tools.cfg import AudibleWarningConfig as Config
+from jsk_tools.diagnostics_utils import diagnostics_level_to_str
 from jsk_tools.diagnostics_utils import filter_diagnostics_status_list
 from jsk_tools.diagnostics_utils import is_leaf
+from jsk_tools.inflection_utils import camel_to_snake
+from jsk_tools.string_utils import multiple_whitespace_to_one
 
 
 def expr_eval(expr):
@@ -33,7 +39,8 @@ def __init__(self, rate=1.0, wait=True,
                  language='',
                  volume=1.0,
                  speak_interval=0,
-                 wait_speak_duration_time=10):
+                 wait_speak_duration_time=10,
+                 diagnostics_level_list=None):
         super(SpeakThread, self).__init__()
         self.wait_speak_duration_time = wait_speak_duration_time
         self.event = Event()
@@ -43,21 +50,66 @@ def __init__(self, rate=1.0, wait=True,
         self.lock = Lock()
         self.status_list = []
         self.speak_interval = speak_interval
+        self.diagnostics_level_list = diagnostics_level_list or []
         tm = rospy.Time.now().to_sec() \
             - speak_interval
         self.previous_spoken_time = defaultdict(lambda tm=tm: tm)
+        self.speak_flag = True
         self.language = language
 
+        self.pub_original_text = rospy.Publisher('~output/original_text',
+                                                 std_msgs.msg.String,
+                                                 queue_size=1)
+        self.pub_speak_text = rospy.Publisher('~output/text',
+                                              std_msgs.msg.String,
+                                              queue_size=1)
+
         self.talk = actionlib.SimpleActionClient(
             "/robotsound", SoundRequestAction)
         self.talk.wait_for_server()
 
     def stop(self):
         self.event.set()
 
+    def set_diagnostics_level_list(self, level_list):
+        self.diagnostics_level_list = level_list
+
+    def set_speak_flag(self, flag):
+        if flag is True and self.speak_flag is False:
+            # clear queue before start speaking.
+            with self.lock:
+                self.status_list = []
+        self.speak_flag = flag
+        if self.speak_flag is True:
+            rospy.loginfo('audible warning is enabled. speak [{}] levels'
+                          .format(', '.join(map(diagnostics_level_to_str,
+                                                self.diagnostics_level_list))))
+        else:
+            rospy.loginfo('audible warning is disabled.')
+
+    def set_volume(self, volume):
+        volume = min(max(0.0, volume), 1.0)
+        if self.volume != volume:
+            self.volume = volume
+            rospy.loginfo("audible warning's volume was set to {}".format(
+                self.volume))
+
+    def set_speak_interval(self, interval):
+        interval = max(0.0, interval)
+        if self.speak_interval != interval:
+            self.speak_interval = interval
+            rospy.loginfo("audible warning's speak interval was set to {}"
+                          .format(self.speak_interval))
+
     def add(self, status_list):
         with self.lock:
             for status in status_list:
+                if is_leaf(status.name) is False:
+                    continue
+                if rospy.Time.now().to_sec() \
+                        - self.previous_spoken_time[status.name] \
+                        < self.speak_interval:
+                    continue
                 heapq.heappush(
                     self.status_list,
                     (rospy.Time.now().to_sec(), status))
@@ -79,7 +131,14 @@ def run(self):
         while not self.event.wait(self.rate):
             e = self.pop()
             if e:
-                if e.level == DiagnosticStatus.WARN:
+                if self.speak_flag is False:
+                    continue
+                if e.level not in self.diagnostics_level_list:
+                    continue
+
+                if e.level == DiagnosticStatus.OK:
+                    prefix = 'ok.'
+                elif e.level == DiagnosticStatus.WARN:
                     prefix = 'warning.'
                 elif e.level == DiagnosticStatus.ERROR:
                     prefix = 'error.'
@@ -88,10 +147,16 @@ def run(self):
                 else:
                     prefix = 'ok.'
                 sentence = prefix + e.name + ' ' + e.message
+                sentence = camel_to_snake(sentence)
                 sentence = sentence.replace('/', ' ')
                 sentence = sentence.replace('_', ' ')
+                sentence = sentence.replace(':', ' colon ')
+                sentence = multiple_whitespace_to_one(sentence)
                 rospy.loginfo('audible warning error name "{}"'.format(e.name))
                 rospy.loginfo("audible warning talking: %s" % sentence)
+                self.pub_original_text.publish(prefix + ' ' + e.name + ' ' + e.message)
+                self.pub_speak_text.publish(
+                    "audible warning talking: %s" % sentence)
 
                 goal = SoundRequestGoal()
                 goal.sound_request.sound = SoundRequest.SAY
@@ -111,30 +176,24 @@ def run(self):
 class AudibleWarning(object):
 
     def __init__(self):
-        speak_rate = rospy.get_param("~speak_rate", 1.0)
-        speak_interval = rospy.get_param("~speak_interval", 120.0)
+        speak_rate = rospy.get_param("~speak_rate", 1.0 / 100.0)
         wait_speak = rospy.get_param("~wait_speak", True)
-        volume = rospy.get_param("~volume", 1.0)
         language = rospy.get_param('~language', '')
         seconds_to_start_speaking = rospy.get_param(
             '~seconds_to_start_speaking', 0)
+        wait_speak_duration_time = rospy.get_param(
+            '~wait_speak_duration_time', 30.0)
 
         # Wait until seconds_to_start_speaking the time has passed.
+        self.run_stop_enabled_time = None
+        self.run_stop_disabled_time = None
         rate = rospy.Rate(10)
         start_time = rospy.Time.now()
         while not rospy.is_shutdown() \
                 and (rospy.Time.now() - start_time).to_sec() \
                 < seconds_to_start_speaking:
             rate.sleep()
 
-        self.diagnostics_list = []
-        if rospy.get_param("~speak_warn", True):
-            self.diagnostics_list.append(DiagnosticStatus.WARN)
-        if rospy.get_param("~speak_error", True):
-            self.diagnostics_list.append(DiagnosticStatus.ERROR)
-        if rospy.get_param("~speak_stale", True):
-            self.diagnostics_list.append(DiagnosticStatus.STALE)
-
         blacklist = rospy.get_param("~blacklist", [])
         self.blacklist_names = []
         self.blacklist_messages = []
@@ -149,13 +208,13 @@ def __init__(self):
             else:
                 message = re.compile(bl['message'])
             self.blacklist_messages.append(message)
-        self.speak_thread = SpeakThread(speak_rate, wait_speak,
-                                        language, volume, speak_interval,
-                                        seconds_to_start_speaking)
+        self.speak_thread = SpeakThread(
+            speak_rate, wait_speak,
+            language,
+            wait_speak_duration_time=wait_speak_duration_time)
+        self.srv = Server(Config, self.config_callback)
 
         # run-stop
-        self.speak_when_runstopped = rospy.get_param(
-            '~speak_when_runstopped', True)
         self.run_stop = False
         self.run_stop_topic = rospy.get_param('~run_stop_topic', None)
         if self.run_stop_topic:
@@ -190,6 +249,27 @@ def __init__(self):
             self.diag_cb, queue_size=1)
         self.speak_thread.start()
 
+    def config_callback(self, config, level):
+        level_list = []
+        if config.speak_ok:
+            level_list.append(DiagnosticStatus.OK)
+        if config.speak_warn:
+            level_list.append(DiagnosticStatus.WARN)
+        if config.speak_error:
+            level_list.append(DiagnosticStatus.ERROR)
+        if config.speak_stale:
+            level_list.append(DiagnosticStatus.STALE)
+        self.speak_thread.set_diagnostics_level_list(level_list)
+        self.speak_thread.set_speak_flag(config.enable)
+        self.speak_thread.set_volume(config.volume)
+        self.speak_thread.set_speak_interval(config.speak_interval)
+        self.ignore_time_after_runstop_is_enabled = \
+            config.ignore_time_after_runstop_is_enabled
+        self.ignore_time_after_runstop_is_disabled = \
+            config.ignore_time_after_runstop_is_disabled
+        self.speak_when_runstopped = config.speak_when_runstopped
+        return config
+
     def run_stop_callback(self, msg):
         if isinstance(msg, rospy.msg.AnyMsg):
             package, msg_type = msg._connection_header['type'].split('/')
@@ -199,18 +279,39 @@ def run_stop_callback(self, msg):
             deserialized_sub = rospy.Subscriber(
                 self.run_stop_topic, msg_class, self.run_stop_callback)
             self.run_stop_sub = deserialized_sub
-            return
-        self.run_stop = self.run_stop_condition(
-            self.run_stop_topic, msg, rospy.Time.now())
+            msg = msg_class().deserialize(msg._buff)
+        tm = rospy.Time.now()
+        run_stop = self.run_stop_condition(
+            self.run_stop_topic, msg, tm)
+        if run_stop != self.run_stop:
+            if run_stop is True:
+                self.run_stop_enabled_time = rospy.Time.now()
+                rospy.loginfo('Audible Warning: Runstop is enabled.')
+            else:
+                self.run_stop_disabled_time = rospy.Time.now()
+                rospy.loginfo('Audible Warning: Runstop is disabled.')
+        self.run_stop = run_stop
 
     def on_shutdown(self):
         self.speak_thread.stop()
         self.speak_thread.join()
 
     def diag_cb(self, msg):
-        target_status_list = filter(
-            lambda n: n.level in self.diagnostics_list,
-            msg.status)
+        target_status_list = msg.status
+
+        if self.ignore_time_after_runstop_is_enabled > 0.0:
+            if self.run_stop_enabled_time is not None \
+                    and ((rospy.Time.now() -
+                          self.run_stop_enabled_time).to_sec() <
+                         self.ignore_time_after_runstop_is_enabled):
+                return
+        if self.ignore_time_after_runstop_is_disabled > 0.0:
+            if self.run_stop_disabled_time is not None \
+                    and ((rospy.Time.now() -
+                          self.run_stop_disabled_time).to_sec() <
+                         self.ignore_time_after_runstop_is_disabled):
+                return
+
         if self.run_stop:
             if self.speak_when_runstopped is False:
                 rospy.logdebug('RUN STOP is pressed. Do not speak warning.')

jsk_tools/package.xml

@@ -15,12 +15,14 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <build_depend>dynamic_reconfigure</build_depend>
   <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>dynamic_reconfigure</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>