From 11a2b1c409ef64431ff06ec05a8041194c93d5f0 Mon Sep 17 00:00:00 2001 From: Naoya Yamaguchi <708yamaguchi@gmail.com> Date: Mon, 19 Jul 2021 21:05:46 +0900 Subject: [PATCH] Do not replace sound when other node is speaking --- jsk_fetch_robot/jsk_fetch.rosinstall.melodic | 1 + .../scripts/battery_warning.py | 26 ++++++--------- .../jsk_robot_startup/scripts/boot_sound.py | 32 ++++++------------- 3 files changed, 19 insertions(+), 40 deletions(-) diff --git a/jsk_fetch_robot/jsk_fetch.rosinstall.melodic b/jsk_fetch_robot/jsk_fetch.rosinstall.melodic index ab049f81076..b64ebfbf4eb 100644 --- a/jsk_fetch_robot/jsk_fetch.rosinstall.melodic +++ b/jsk_fetch_robot/jsk_fetch.rosinstall.melodic @@ -71,6 +71,7 @@ local-name: mikeferguson/robot_calibration uri: https://github.com/mikeferguson/robot_calibration.git version: 0.5.5 +# https://github.com/ros-drivers/audio_common/pull/173 - git: local-name: ros-drivers/audio_common uri: https://github.com/ros-drivers/audio_common.git diff --git a/jsk_fetch_robot/jsk_fetch_startup/scripts/battery_warning.py b/jsk_fetch_robot/jsk_fetch_startup/scripts/battery_warning.py index e4de6a2d3f5..b61a672c860 100755 --- a/jsk_fetch_robot/jsk_fetch_startup/scripts/battery_warning.py +++ b/jsk_fetch_robot/jsk_fetch_startup/scripts/battery_warning.py @@ -3,19 +3,15 @@ import actionlib import rospy +from sound_play.libsoundplay import SoundClient from power_msgs.msg import BatteryState -from sound_play.msg import SoundRequest -from sound_play.msg import SoundRequestAction -from sound_play.msg import SoundRequestGoal class BatteryWarning(object): def __init__(self): - self.client_en = actionlib.SimpleActionClient( - '/sound_play', SoundRequestAction) - self.client_jp = actionlib.SimpleActionClient( - '/robotsound_jp', SoundRequestAction) + self.client_en = SoundClient(sound_action='/sound_play', blocking=True) + self.client_jp = SoundClient(sound_action='/robotsound_jp', blocking=True) self.duration = rospy.get_param('~duration', 180.0) self.threshold = rospy.get_param('~charge_level_threshold', 40.0) self.step = rospy.get_param('~charge_level_step', 10.0) @@ -28,17 +24,13 @@ def __init__(self): self.is_charging = False def _speak(self, client, speech_text, lang=None): - client.wait_for_server(timeout=rospy.Duration(1.0)) - sound_goal = SoundRequestGoal() - sound_goal.sound_request.sound = -3 - sound_goal.sound_request.command = 1 - sound_goal.sound_request.volume = self.volume + client.actionclient.wait_for_server(timeout=rospy.Duration(1.0)) if lang is not None: - sound_goal.sound_request.arg2 = lang - sound_goal.sound_request.arg = speech_text - client.send_goal(sound_goal) - client.wait_for_result() - return client.get_result() + client.say(speech_text, voice=lang, replace=False) + else: + client.say(speech_text, replace=False) + client.actionclient.wait_for_result() + return client.actionclient.get_result() def _warn(self): if self.charge_level < self.threshold and not self.is_charging: diff --git a/jsk_robot_common/jsk_robot_startup/scripts/boot_sound.py b/jsk_robot_common/jsk_robot_startup/scripts/boot_sound.py index f5abb37745b..5117f922c64 100755 --- a/jsk_robot_common/jsk_robot_startup/scripts/boot_sound.py +++ b/jsk_robot_common/jsk_robot_startup/scripts/boot_sound.py @@ -3,14 +3,10 @@ # https://www.youtube.com/watch?v=HM-xG0qXaeA&& # ffmpeg -i R2D2\ all\ Sounds\ -\ Star\ Wars\ free\ sounds.mp4 -ss 48 -t 10 R2D2.wav -import rospy -import time, socket, os import netifaces as ni -import rospkg +import rospy from sound_play.libsoundplay import SoundClient -import actionlib -from sound_play.msg import SoundRequestAction if __name__ == "__main__": rospy.init_node("boot_sound") @@ -19,32 +15,22 @@ else: wav_file = "/usr/share/sounds/alsa/Front_Center.wav" - sound = SoundClient() - time.sleep(1) # ??? - ac = actionlib.SimpleActionClient('sound_play', SoundRequestAction) - ac.wait_for_server() + sound = SoundClient(sound_action='sound_play', blocking=True) + sound.actionclient.wait_for_server() - - interfaces = [x for x in ni.interfaces() if x[0:3] in ['eth', 'enp', 'wla', 'wlp'] and - 2 in ni.ifaddresses(x).keys()] - if len(interfaces) > 0 : - ip = ni.ifaddresses(interfaces[0])[2][0]['addr'] + if len(ni.ifaddresses('eth0')) > 2: + ip = ni.ifaddresses('eth0')[2][0]['addr'] + elif len(ni.ifaddresses('wlan0')) > 2: + ip = ni.ifaddresses('wlan0')[2][0]['addr'] else: ip = None # play sound rospy.loginfo("Playing {}".format(wav_file)) - sound.playWave(wav_file) - time.sleep(10) # make sure to topic is going out + sound.playWave(wav_file, replace=False) # notify ip address ip_text = "My internet address is {}".format(ip) rospy.loginfo(ip_text) ip_text = ip_text.replace('.', ', ') - sound.say(ip_text) - time.sleep(1) # make sure to topic is going out - - - - - + sound.say(ip_text, replace=False)