Skip to content

Commit

Permalink
Merge pull request #22 from iory/pr1040-audible-warning
Browse files Browse the repository at this point in the history
  • Loading branch information
708yamaguchi authored Jul 7, 2022
2 parents 5b85a62 + 558cd86 commit b90ac37
Show file tree
Hide file tree
Showing 12 changed files with 292 additions and 32 deletions.
24 changes: 22 additions & 2 deletions doc/jsk_tools/scripts/audible_warning.md
Original file line number Diff line number Diff line change
Expand Up @@ -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`)

Expand All @@ -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.
Expand Down Expand Up @@ -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.
Expand Down
15 changes: 11 additions & 4 deletions jsk_tools/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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 #
Expand All @@ -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)
Expand Down Expand Up @@ -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
)
Expand Down
26 changes: 26 additions & 0 deletions jsk_tools/cfg/AudibleWarning.cfg
Original file line number Diff line number Diff line change
@@ -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"))
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand All @@ -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()
Expand All @@ -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))
Expand All @@ -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.'
Expand All @@ -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
Expand All @@ -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 = []
Expand All @@ -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:
Expand Down Expand Up @@ -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('/')
Expand All @@ -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.')
Expand Down
2 changes: 2 additions & 0 deletions jsk_tools/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
Loading

0 comments on commit b90ac37

Please sign in to comment.