diff --git a/doc/jsk_tools/scripts/audible_warning.md b/doc/jsk_tools/scripts/audible_warning.md new file mode 100644 index 000000000..95fb4d3a9 --- /dev/null +++ b/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 ``. + +## 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: + + ``` + + + + + 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/.*" + + + ``` + +- `~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) diff --git a/doc/jsk_tools/scripts/images/audible_warning.jpg b/doc/jsk_tools/scripts/images/audible_warning.jpg new file mode 100644 index 000000000..5d3ed1f7d Binary files /dev/null and b/doc/jsk_tools/scripts/images/audible_warning.jpg differ diff --git a/jsk_tools/CMakeLists.txt b/jsk_tools/CMakeLists.txt index c1a9e6779..451998060 100644 --- a/jsk_tools/CMakeLists.txt +++ b/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 ) diff --git a/jsk_tools/cfg/AudibleWarning.cfg b/jsk_tools/cfg/AudibleWarning.cfg new file mode 100755 index 000000000..705682213 --- /dev/null +++ b/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")) diff --git a/jsk_tools/env-hooks/99.jsk_tools.sh b/jsk_tools/env-hooks/99.jsk_tools.sh index c07ec0fb4..22e2ccdfb 100644 --- a/jsk_tools/env-hooks/99.jsk_tools.sh +++ b/jsk_tools/env-hooks/99.jsk_tools.sh @@ -44,7 +44,11 @@ _update_prompt() { export PS1="${WITHOUT_ROS_PROMPT}" fi elif [ "$master_host" != "" ]; then - local ros_prompt="[$ROS_MASTER_URI][$ROS_IP]" + if [[ "$ROS_HOSTNAME" != "" && "$ROS_IP" == "" ]]; then + local ros_prompt="[$ROS_MASTER_URI][$ROS_HOSTNAME]" + else + local ros_prompt="[$ROS_MASTER_URI][$ROS_IP]" + fi if [ "$CATKIN_SHELL" = "bash" ]; then export PS1="\[\033[00;31m\]$ros_prompt\[\033[00m\] ${WITHOUT_ROS_PROMPT}" elif [ "$CATKIN_SHELL" = "zsh" ]; then @@ -86,6 +90,7 @@ rossetip_dev() { rossetip_addr() { local target_host=${1-"133.11.216.211"} + echo "target_host: $target_host" # Check if target_host looks like ip address or not if [ "$(echo $target_host | sed -e 's/[0-9\.]//g')" != "" ]; then target_host_ip=$(timeout 0.01 getent hosts ${target_host} | cut -f 1 -d ' ') @@ -125,6 +130,24 @@ rossetip() { fi } +rossetclient() { + local client_interface=$1 + if [[ $client_interface =~ [0-9]+.[0-9]+.[0-9]+.[0-9]+ ]]; then + export ROS_IP="$client_interface" + else + local addr_if_client_is_device=${LANC=C ip -o -4 a | grep lo\ \ \ | cut -d\ -f7 | cut -d\/ -f1} + if [[ $addr_if_client_is_device = "" ]]; then + export ROS_IP="$addr_if_client_is_device" + else + export ROS_HOSTNAME="$client_interface" + fi + fi + echo -e "\e[1;31mset ROS_IP to \"$ROS_IP\" and ROS_HOSTNAME to \"$ROS_HOSTNAME\"\e[m" + if [ "$NO_ROS_PROMPT" = "" ]; then + _update_prompt + fi +} + rosn() { if [ "$1" = "" ]; then select=$(rosnode list | percol | xargs -n 1 rosnode info | percol) diff --git a/jsk_tools/node_scripts/audible_warning.py b/jsk_tools/node_scripts/audible_warning.py new file mode 100755 index 000000000..f1bd39845 --- /dev/null +++ b/jsk_tools/node_scripts/audible_warning.py @@ -0,0 +1,334 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +from collections import defaultdict +import heapq +from importlib import import_module +import re +from threading import Event +from threading import Lock +from threading import Thread + +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): + def eval_fn(topic, m, t): + return eval(expr) + return eval_fn + + +class SpeakThread(Thread): + + def __init__(self, rate=1.0, wait=True, + language='', + volume=1.0, + speak_interval=0, + 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() + self.rate = rate + self.wait = wait + self.volume = volume + 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)) + + def pop(self): + with self.lock: + while len(self.status_list) > 0: + _, status = heapq.heappop(self.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 + return status + return None + + def run(self): + while not self.event.wait(self.rate): + e = self.pop() + if e: + 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.' + elif e.level == DiagnosticStatus.STALE: + prefix = 'stale.' + 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 + goal.sound_request.command = SoundRequest.PLAY_ONCE + goal.sound_request.arg = sentence + goal.sound_request.arg2 = self.language + if hasattr(goal.sound_request, 'volume'): + goal.sound_request.volume = self.volume + + self.previous_spoken_time[e.name] = rospy.Time.now().to_sec() + self.talk.send_goal(goal) + if self.wait: + self.talk.wait_for_result( + rospy.Duration(self.wait_speak_duration_time)) + + +class AudibleWarning(object): + + def __init__(self): + speak_rate = rospy.get_param("~speak_rate", 1.0 / 100.0) + wait_speak = rospy.get_param("~wait_speak", True) + 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() + + blacklist = rospy.get_param("~blacklist", []) + self.blacklist_names = [] + self.blacklist_messages = [] + for bl in blacklist: + if 'name' not in bl: + name = re.compile(r'.') + else: + name = re.compile(bl['name']) + self.blacklist_names.append(name) + if 'message' not in bl: + message = re.compile(r'.') + else: + message = re.compile(bl['message']) + self.blacklist_messages.append(message) + 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.run_stop = False + self.run_stop_topic = rospy.get_param('~run_stop_topic', None) + if self.run_stop_topic: + run_stop_condition = rospy.get_param( + '~run_stop_condition', 'm.data == True') + run_stop_blacklist = rospy.get_param( + '~run_stop_blacklist', []) + self.run_stop_blacklist_names = [] + self.run_stop_blacklist_messages = [] + for bl in run_stop_blacklist: + if 'name' not in bl: + name = re.compile(r'.') + else: + name = re.compile(bl['name']) + self.run_stop_blacklist_names.append(name) + if 'message' not in bl: + message = re.compile(r'.') + else: + message = re.compile(bl['message']) + self.run_stop_blacklist_messages.append(message) + self.run_stop_condition = expr_eval(run_stop_condition) + self.run_stop_sub = rospy.Subscriber( + self.run_stop_topic, + rospy.AnyMsg, + + callback=self.run_stop_callback, + queue_size=1) + + # diag + self.sub_diag = rospy.Subscriber( + "/diagnostics_agg", DiagnosticArray, + 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('/') + ros_pkg = package + '.msg' + msg_class = getattr(import_module(ros_pkg), msg_type) + self.run_stop_sub.unregister() + deserialized_sub = rospy.Subscriber( + self.run_stop_topic, msg_class, self.run_stop_callback) + self.run_stop_sub = deserialized_sub + 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 = 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.') + return + + target_status_list = filter_diagnostics_status_list( + target_status_list, + self.run_stop_blacklist_names, + self.run_stop_blacklist_messages) + + target_status_list = filter_diagnostics_status_list( + target_status_list, self.blacklist_names, self.blacklist_messages) + self.speak_thread.add(target_status_list) + + +if __name__ == '__main__': + rospy.init_node("audible_warning") + aw = AudibleWarning() # NOQA + rospy.on_shutdown(aw.on_shutdown) + rospy.spin() diff --git a/jsk_tools/package.xml b/jsk_tools/package.xml index ef0a9d455..082dc1b2b 100644 --- a/jsk_tools/package.xml +++ b/jsk_tools/package.xml @@ -15,12 +15,14 @@ catkin + dynamic_reconfigure git rosgraph_msgs cv_bridge diagnostic_aggregator diagnostic_msgs diagnostic_updater + dynamic_reconfigure python-percol python-colorama python3-colorama diff --git a/jsk_tools/sample/config/sample_audible_warning_diagnostics_analyzer.yaml b/jsk_tools/sample/config/sample_audible_warning_diagnostics_analyzer.yaml new file mode 100644 index 000000000..c6c0feb93 --- /dev/null +++ b/jsk_tools/sample/config/sample_audible_warning_diagnostics_analyzer.yaml @@ -0,0 +1,15 @@ +analyzers: + 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 diff --git a/jsk_tools/sample/pseudo_diagnostics.py b/jsk_tools/sample/pseudo_diagnostics.py new file mode 100755 index 000000000..92f0b4d4c --- /dev/null +++ b/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() diff --git a/jsk_tools/sample/pseudo_runstop_publisher.py b/jsk_tools/sample/pseudo_runstop_publisher.py new file mode 100755 index 000000000..7091fc95f --- /dev/null +++ b/jsk_tools/sample/pseudo_runstop_publisher.py @@ -0,0 +1,41 @@ +#!/usr/bin/env python + +from distutils.version import LooseVersion as version + +import pkg_resources +import rospy +import std_msgs.msg + + +class PseudoRunstopPublisher(object): + + def __init__(self): + duration_time = rospy.get_param('~duration_time', 30) + self.runstop = True + self.pub = rospy.Publisher('~runstop', std_msgs.msg.Bool, + queue_size=1) + kwargs = dict( + period=rospy.Duration(duration_time), + callback=self.publish_callback, + 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 publish_callback(self, event): + if self.runstop is True: + rospy.loginfo('Runstop is enabled.') + else: + rospy.loginfo('Runstop is disabled.') + self.pub.publish(self.runstop) + self.runstop = not self.runstop + + +if __name__ == '__main__': + rospy.init_node('pseudo_runstop_publisher') + pub = PseudoRunstopPublisher() + rospy.spin() diff --git a/jsk_tools/sample/sample_audible_warning.launch b/jsk_tools/sample/sample_audible_warning.launch new file mode 100644 index 000000000..f9fb14ed0 --- /dev/null +++ b/jsk_tools/sample/sample_audible_warning.launch @@ -0,0 +1,53 @@ + + + + + + + + + + + + duration_time: 30 + + + + + + + + + + + + + + + + run_stop_topic: /pseudo_runstop_publisher/runstop + speak_when_runstopped: true + speak_interval: 0.0 + seconds_to_start_speaking: 5 + ignore_time_after_runstop_is_enabled: 10.0 + ignore_time_after_runstop_is_disabled: 10.0 + + + + + + + + + diff --git a/jsk_tools/src/jsk_tools/clients/__init__.py b/jsk_tools/src/jsk_tools/clients/__init__.py new file mode 100644 index 000000000..91a96e71c --- /dev/null +++ b/jsk_tools/src/jsk_tools/clients/__init__.py @@ -0,0 +1 @@ +from .audible_warninig_client import AudibleWarningReconfigureClient diff --git a/jsk_tools/src/jsk_tools/clients/audible_warninig_client.py b/jsk_tools/src/jsk_tools/clients/audible_warninig_client.py new file mode 100644 index 000000000..0bc575505 --- /dev/null +++ b/jsk_tools/src/jsk_tools/clients/audible_warninig_client.py @@ -0,0 +1,18 @@ +from dynamic_reconfigure.client import Client + + +class AudibleWarningReconfigureClient(object): + + def __init__(self, client_name='audible_warning'): + super(AudibleWarningReconfigureClient).__init__() + self.client_name = client_name + self.client = Client(client_name, timeout=3) + + def reconfigure(self, **kwargs): + return self.client.update_configuration(kwargs) + + def enable(self): + return self.client.update_configuration({'enable': True}) + + def disable(self): + return self.client.update_configuration({'enable': False}) diff --git a/jsk_tools/src/jsk_tools/diagnostics_utils.py b/jsk_tools/src/jsk_tools/diagnostics_utils.py new file mode 100644 index 000000000..a65c506bc --- /dev/null +++ b/jsk_tools/src/jsk_tools/diagnostics_utils.py @@ -0,0 +1,92 @@ +try: + from itertools import zip_longest +except: + from itertools import izip_longest as zip_longest +import re + +from diagnostic_msgs.msg import DiagnosticStatus + + +cached_paths = {} +cached_result = {} + + +def is_leaf(original_path): + if hasattr(cached_result, original_path): + return cached_result[original_path] + + path = original_path[1:].split('/') + + def _is_leaf(cached, path): + if not isinstance(path, list): + raise ValueError + if len(path) == 0: + raise ValueError + if len(path) == 1: + if path[0] in cached: + return False + else: + return True + + parent = path[0] + child = path[1:] + if parent not in cached: + cached[parent] = {} + return _is_leaf(cached[parent], child) + + result = _is_leaf(cached_paths, path) + if result is False: + cached_result[original_path] = result + return result + + +def filter_diagnostics_status_list(status_list, blacklist, + blacklist_messages=None): + """Filter list of DiagnosticStatus. + + Parameters + ---------- + status_list : List[DiagnosticStatus] + List of DiagnosticStatus. + blacklist : List[str] or List[re.Pattern] + List of blacklist. + The path of the name contained in this list is ignored. + blacklist_messages : List[str] or List[re.Pattern] + List of blacklist for message. + This has a one-to-one correspondence with blacklist. + + Returns + ------- + filtered_status : List[DiagnosticStatus] + List of filtered diagnostics status. + """ + blacklist_messages = blacklist_messages or [] + filtered_status = [] + for s in status_list: + ns = s.name + if is_leaf(ns) is False: + continue + matched = False + for bn, message in zip_longest( + blacklist, blacklist_messages): + if re.match(bn, ns): + if message is None or re.match(message, s.message): + matched = True + break + if matched is True: + continue + filtered_status.append(s) + return filtered_status + + +def diagnostics_level_to_str(level): + if level == DiagnosticStatus.OK: + return 'OK' + elif level == DiagnosticStatus.WARN: + return 'WARN' + elif level == DiagnosticStatus.ERROR: + return 'ERROR' + elif level == DiagnosticStatus.STALE: + return 'STALE' + else: + raise ValueError('Not valid level {}'.format(level)) diff --git a/jsk_tools/src/jsk_tools/inflection_utils.py b/jsk_tools/src/jsk_tools/inflection_utils.py new file mode 100644 index 000000000..455869920 --- /dev/null +++ b/jsk_tools/src/jsk_tools/inflection_utils.py @@ -0,0 +1,15 @@ +import re + + +def camel_to_snake(word): + """Convert Camel case to snake case. + + Examples + -------- + >>> camel_to_snake('clusterPointIndices') + 'cluster_point_indices' + """ + word = re.sub(r"([A-Z]+)([A-Z][a-z])", r'\1_\2', word) + word = re.sub(r"([a-z\d])([A-Z])", r'\1_\2', word) + word = word.replace("-", "_") + return word.lower() diff --git a/jsk_tools/src/jsk_tools/string_utils.py b/jsk_tools/src/jsk_tools/string_utils.py new file mode 100644 index 000000000..fe9459ce5 --- /dev/null +++ b/jsk_tools/src/jsk_tools/string_utils.py @@ -0,0 +1,2 @@ +def multiple_whitespace_to_one(text): + return ' '.join(text.split()) diff --git a/jsk_tools/test/test_audible_warning.test b/jsk_tools/test/test_audible_warning.test new file mode 100644 index 000000000..9efa2e8b3 --- /dev/null +++ b/jsk_tools/test/test_audible_warning.test @@ -0,0 +1,17 @@ + + + + + + + + + + topic_0: /sound_play/goal + timeout_0: 10 + + + +