def _start_playing(self, gh, result):
# Now open a file pointer to the wav files so that we can be sure the
# file exists while play has access to it
try:
fd = open(gh.get_goal().sound_request.arg, 'rb')
except Exception as e:
rospy.logerr("Exception while opening wav file: {}".format(e))
result.stamp = rospy.Time.now()
gh.set_aborted(result)
# We now have access to the file pointer. Now send it on to play (or
# whatever command we have chosen to play sounds)
play_command = ([SoundServer.DEFAULT_SOUNDPLAY_COMMAND] +
SoundServer.DEFAULT_SOUNDPLAY_ARGS +
[gh.get_goal().sound_request.arg])
play_process = psutil.Popen(play_command)
self._current_sounds.put((
gh,
play_process,
rospy.Time.now(),
))
gh.publish_feedback(
SoundRequestFeedback(playing=True, stamp=rospy.Time.now()))
# Now wait until we receive a signal that the process has terminated
while play_process.poll() is None:
rospy.sleep(0.1)
# Then check the status we were in. Close the file descriptor first
fd.close()
if gh.get_goal_status().status == GoalStatus.PREEMPTING:
rospy.logwarn("Goal to play {} preempted.".format(
gh.get_goal().sound_request.arg))
result.stamp = rospy.Time.now()
gh.set_canceled(result)
elif play_process.poll() != 0:
rospy.logerr("Goal to play {} aborted.".format(
gh.get_goal().sound_request.arg))
result.stamp = rospy.Time.now()
gh.set_aborted(result)
else: # status == SUCCEEDED and poll() == 0
rospy.logdebug("Goal to play {} succeeded.".format(
gh.get_goal().sound_request.arg))
result.stamp = rospy.Time.now()
gh.set_succeeded(result)
class soundplay:
_feedback = SoundRequestFeedback()
_result = SoundRequestResult()
def stopdict(self, dict):
for sound in dict.values():
sound.stop()
def stopall(self):
self.stopdict(self.builtinsounds)
self.stopdict(self.filesounds)
self.stopdict(self.voicesounds)
def select_sound(self, data):
if data.sound == SoundRequest.PLAY_FILE:
if not data.arg2:
if not data.arg in self.filesounds.keys() or self.filesounds[
data.arg].volume != data.volume:
rospy.logdebug('command for uncached wave: "%s"' %
data.arg)
try:
self.filesounds[data.arg] = soundtype(
data.arg, data.volume)
except:
rospy.logerr(
'Error setting up to play "%s". Does this file exist on the machine on which sound_play is running?'
% data.arg)
return
else:
rospy.logdebug('command for cached wave: "%s"' % data.arg)
sound = self.filesounds[data.arg]
else:
absfilename = os.path.join(
roslib.packages.get_pkg_dir(data.arg2), data.arg)
if not absfilename in self.filesounds.keys(
) or self.filesounds[absfilename].volume != data.volume:
rospy.logdebug('command for uncached wave: "%s"' %
absfilename)
try:
self.filesounds[absfilename] = soundtype(
absfilename, data.volume)
except:
rospy.logerr(
'Error setting up to play "%s" from package "%s". Does this file exist on the machine on which sound_play is running?'
% (data.arg, data.arg2))
return
else:
rospy.logdebug('command for cached wave: "%s"' %
absfilename)
sound = self.filesounds[absfilename]
elif data.sound == SoundRequest.SAY:
print data
if not data.arg in self.voicesounds.keys() or self.voicesounds[
data.arg].volume != data.volume:
rospy.logdebug('command for uncached text: "%s"' % data.arg)
txtfile = tempfile.NamedTemporaryFile(prefix='sound_play',
suffix='.txt')
(wavfile, wavfilename) = tempfile.mkstemp(prefix='sound_play',
suffix='.wav')
txtfilename = txtfile.name
os.close(wavfile)
voice = data.arg2
try:
txtfile.write(data.arg)
txtfile.flush()
os.system("text2wave -eval '(" + voice + ")' " +
txtfilename + " -o " + wavfilename)
try:
if os.stat(wavfilename).st_size == 0:
raise OSError # So we hit the same catch block
except OSError:
rospy.logerr(
'Sound synthesis failed. Is festival installed? Is a festival voice installed? Try running "rosdep satisfy sound_play|sh". Refer to http://wiki.ros.org/sound_play/Troubleshooting'
)
return
self.voicesounds[data.arg] = soundtype(
wavfilename, data.volume)
finally:
txtfile.close()
else:
rospy.logdebug('command for cached text: "%s"' % data.arg)
sound = self.voicesounds[data.arg]
else:
rospy.logdebug('command for builtin wave: %i' % data.sound)
if data.sound not in self.builtinsounds or (
data.sound in self.builtinsounds
and data.volume != self.builtinsounds[data.sound].volume):
params = self.builtinsoundparams[data.sound]
volume = data.volume
if params[
1] != 1: # use the second param as a scaling for the input volume
volume = (volume + params[1]) / 2
self.builtinsounds[data.sound] = soundtype(params[0], volume)
sound = self.builtinsounds[data.sound]
if sound.staleness != 0 and data.command != SoundRequest.PLAY_STOP:
# This sound isn't counted in active_sounds
rospy.logdebug("activating %i %s" % (data.sound, data.arg))
self.active_sounds = self.active_sounds + 1
sound.staleness = 0
# if self.active_sounds > self.num_channels:
# mixer.set_num_channels(self.active_sounds)
# self.num_channels = self.active_sounds
return sound
def callback(self, data):
if not self.initialized:
return
self.mutex.acquire()
# Force only one sound at a time
self.stopall()
try:
if data.sound == SoundRequest.ALL and data.command == SoundRequest.PLAY_STOP:
self.stopall()
else:
sound = self.select_sound(data)
sound.command(data.command)
#pub_sound = rospy.Publisher("sout", String, queue_size=2)
pub_sound.publish("o")
rospy.loginfo("sound Check")
except Exception, e:
rospy.logerr('Exception in callback: %s' % str(e))
rospy.loginfo(traceback.format_exc())
finally:
class soundplay:
_feedback = SoundRequestFeedback()
_result = SoundRequestResult()
def stopdict(self, dict):
for sound in dict.values():
sound.stop()
def stopall(self):
self.stopdict(self.builtinsounds)
self.stopdict(self.filesounds)
self.stopdict(self.voicesounds)
def select_sound(self, data):
if data.sound == SoundRequest.PLAY_FILE:
if not data.arg2:
if not data.arg in self.filesounds.keys():
rospy.logdebug('command for uncached wave: "%s"' %
data.arg)
try:
self.filesounds[data.arg] = soundtype(
data.arg, self.device, data.volume)
except:
rospy.logerr(
'Error setting up to play "%s". Does this file exist on the machine on which sound_play is running?'
% data.arg)
return
else:
rospy.logdebug('command for cached wave: "%s"' % data.arg)
if self.filesounds[data.arg].sound.get_property(
'volume') != data.volume:
rospy.logdebug(
'volume for cached wave has changed, resetting volume'
)
self.filesounds[data.arg].sound.set_property(
'volume', data.volume)
sound = self.filesounds[data.arg]
else:
absfilename = os.path.join(
roslib.packages.get_pkg_dir(data.arg2), data.arg)
if not absfilename in self.filesounds.keys():
rospy.logdebug('command for uncached wave: "%s"' %
absfilename)
try:
self.filesounds[absfilename] = soundtype(
absfilename, self.device, data.volume)
except:
rospy.logerr(
'Error setting up to play "%s" from package "%s". Does this file exist on the machine on which sound_play is running?'
% (data.arg, data.arg2))
return
else:
rospy.logdebug('command for cached wave: "%s"' %
absfilename)
if self.filesounds[absfilename].sound.get_property(
'volume') != data.volume:
rospy.logdebug(
'volume for cached wave has changed, resetting volume'
)
self.filesounds[absfilename].sound.set_property(
'volume', data.volume)
sound = self.filesounds[absfilename]
elif data.sound == SoundRequest.SAY:
# print data
if not data.arg in self.voicesounds.keys():
rospy.logdebug('command for uncached text: "%s"' % data.arg)
txtfile = tempfile.NamedTemporaryFile(prefix='sound_play',
suffix='.txt')
(wavfile, wavfilename) = tempfile.mkstemp(prefix='sound_play',
suffix='.wav')
txtfilename = txtfile.name
os.close(wavfile)
voice = data.arg2
try:
try:
if hasattr(data.arg, 'decode'):
txtfile.write(
data.arg.decode('UTF-8').encode('ISO-8859-15'))
else:
txtfile.write(data.arg.encode('ISO-8859-15'))
except UnicodeEncodeError:
if hasattr(data.arg, 'decode'):
txtfile.write(data.arg)
else:
txtfile.write(data.arg.encode('UTF-8'))
txtfile.flush()
os.system("text2wave -eval '(" + voice + ")' " +
txtfilename + " -o " + wavfilename)
try:
if os.stat(wavfilename).st_size == 0:
raise OSError # So we hit the same catch block
except OSError:
rospy.logerr(
'Sound synthesis failed. Is festival installed? Is a festival voice installed? Try running "rosdep satisfy sound_play|sh". Refer to http://wiki.ros.org/sound_play/Troubleshooting'
)
return
self.voicesounds[data.arg] = soundtype(
wavfilename, self.device, data.volume)
finally:
txtfile.close()
else:
rospy.logdebug('command for cached text: "%s"' % data.arg)
if self.voicesounds[data.arg].sound.get_property(
'volume') != data.volume:
rospy.logdebug(
'volume for cached text has changed, resetting volume')
self.voicesounds[data.arg].sound.set_property(
'volume', data.volume)
sound = self.voicesounds[data.arg]
else:
rospy.logdebug('command for builtin wave: %i' % data.sound)
if data.sound not in self.builtinsounds or (
data.sound in self.builtinsounds
and data.volume != self.builtinsounds[data.sound].volume):
params = self.builtinsoundparams[data.sound]
volume = data.volume
if params[
1] != 1: # use the second param as a scaling for the input volume
volume = (volume + params[1]) / 2
self.builtinsounds[data.sound] = soundtype(
params[0], self.device, volume)
sound = self.builtinsounds[data.sound]
if sound.staleness != 0 and data.command != SoundRequest.PLAY_STOP:
# This sound isn't counted in active_sounds
rospy.logdebug("activating %i %s" % (data.sound, data.arg))
self.active_sounds = self.active_sounds + 1
sound.staleness = 0
# if self.active_sounds > self.num_channels:
# mixer.set_num_channels(self.active_sounds)
# self.num_channels = self.active_sounds
return sound
def callback(self, data):
if not self.initialized:
return
self.mutex.acquire()
try:
if data.sound == SoundRequest.ALL and data.command == SoundRequest.PLAY_STOP:
self.stopall()
else:
sound = self.select_sound(data)
sound.command(data.command)
except Exception as e:
rospy.logerr('Exception in callback: %s' % str(e))
rospy.loginfo(traceback.format_exc())
finally:
self.mutex.release()
rospy.logdebug("done callback")
# Purge sounds that haven't been played in a while.
def cleanupdict(self, dict):
purgelist = []
for (key, sound) in iter(dict.items()):
try:
staleness = sound.get_staleness()
except Exception as e:
rospy.logerr('Exception in cleanupdict for sound (%s): %s' %
(str(key), str(e)))
staleness = 100 # Something is wrong. Let's purge and try again.
#print "%s %i"%(key, staleness)
if staleness >= 10:
purgelist.append(key)
if staleness == 0: # Sound is playing
self.active_sounds = self.active_sounds + 1
for key in purgelist:
rospy.logdebug('Purging %s from cache' % key)
dict[key].dispose() # clean up resources
del dict[key]
def cleanup(self):
self.mutex.acquire()
try:
self.active_sounds = 0
self.cleanupdict(self.filesounds)
self.cleanupdict(self.voicesounds)
self.cleanupdict(self.builtinsounds)
except:
rospy.loginfo('Exception in cleanup: %s' % sys.exc_info()[0])
finally:
self.mutex.release()
def diagnostics(self, state):
try:
da = DiagnosticArray()
ds = DiagnosticStatus()
ds.name = rospy.get_caller_id().lstrip('/') + ": Node State"
if state == 0:
ds.level = DiagnosticStatus.OK
ds.message = "%i sounds playing" % self.active_sounds
ds.values.append(
KeyValue("Active sounds", str(self.active_sounds)))
ds.values.append(
KeyValue("Allocated sound channels",
str(self.num_channels)))
ds.values.append(
KeyValue("Buffered builtin sounds",
str(len(self.builtinsounds))))
ds.values.append(
KeyValue("Buffered wave sounds",
str(len(self.filesounds))))
ds.values.append(
KeyValue("Buffered voice sounds",
str(len(self.voicesounds))))
elif state == 1:
ds.level = DiagnosticStatus.WARN
ds.message = "Sound device not open yet."
else:
ds.level = DiagnosticStatus.ERROR
ds.message = "Can't open sound device. See http://wiki.ros.org/sound_play/Troubleshooting"
da.status.append(ds)
da.header.stamp = rospy.get_rostime()
self.diagnostic_pub.publish(da)
except Exception as e:
rospy.loginfo('Exception in diagnostics: %s' % str(e))
def execute_cb(self, data):
data = data.sound_request
if not self.initialized:
return
self.mutex.acquire()
# Force only one sound at a time
self.stopall()
try:
if data.sound == SoundRequest.ALL and data.command == SoundRequest.PLAY_STOP:
self.stopall()
else:
sound = self.select_sound(data)
sound.command(data.command)
r = rospy.Rate(1)
start_time = rospy.get_rostime()
success = True
while sound.get_playing():
sound.update()
if self._as.is_preempt_requested():
rospy.loginfo('sound_play action: Preempted')
sound.stop()
self._as.set_preempted()
success = False
break
self._feedback.playing = sound.get_playing()
self._feedback.stamp = rospy.get_rostime() - start_time
self._as.publish_feedback(self._feedback)
r.sleep()
if success:
self._result.playing = self._feedback.playing
self._result.stamp = self._feedback.stamp
rospy.loginfo('sound_play action: Succeeded')
self._as.set_succeeded(self._result)
except Exception as e:
rospy.logerr('Exception in actionlib callback: %s' % str(e))
rospy.loginfo(traceback.format_exc())
finally:
self.mutex.release()
rospy.logdebug("done actionlib callback")
def __init__(self):
Gst.init(None)
# Start gobject thread to receive gstreamer messages
GObject.threads_init()
self.g_loop = threading.Thread(target=GObject.MainLoop().run)
self.g_loop.daemon = True
self.g_loop.start()
rospy.init_node('sound_play')
self.device = rospy.get_param("~device", "default")
self.diagnostic_pub = rospy.Publisher("/diagnostics",
DiagnosticArray,
queue_size=1)
rootdir = os.path.join(roslib.packages.get_pkg_dir('sound_play'),
'sounds')
self.builtinsoundparams = {
SoundRequest.BACKINGUP: (os.path.join(rootdir,
'BACKINGUP.ogg'), 0.1),
SoundRequest.NEEDS_UNPLUGGING:
(os.path.join(rootdir, 'NEEDS_UNPLUGGING.ogg'), 1),
SoundRequest.NEEDS_PLUGGING:
(os.path.join(rootdir, 'NEEDS_PLUGGING.ogg'), 1),
SoundRequest.NEEDS_UNPLUGGING_BADLY:
(os.path.join(rootdir, 'NEEDS_UNPLUGGING_BADLY.ogg'), 1),
SoundRequest.NEEDS_PLUGGING_BADLY:
(os.path.join(rootdir, 'NEEDS_PLUGGING_BADLY.ogg'), 1),
}
self.no_error = True
self.initialized = False
self.active_sounds = 0
self.mutex = threading.Lock()
sub = rospy.Subscriber("robotsound", SoundRequest, self.callback)
self._as = actionlib.SimpleActionServer('sound_play',
SoundRequestAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
self.mutex.acquire()
self.sleep(0.5) # For ros startup race condition
self.diagnostics(1)
while not rospy.is_shutdown():
while not rospy.is_shutdown():
self.init_vars()
self.no_error = True
self.initialized = True
self.mutex.release()
try:
self.idle_loop()
# Returns after inactive period to test device availability
#print "Exiting idle"
except:
rospy.loginfo('Exception in idle_loop: %s' %
sys.exc_info()[0])
finally:
self.mutex.acquire()
self.diagnostics(2)
self.mutex.release()
def init_vars(self):
self.num_channels = 10
self.builtinsounds = {}
self.filesounds = {}
self.voicesounds = {}
self.hotlist = []
if not self.initialized:
rospy.loginfo('sound_play node is ready to play sound')
def sleep(self, duration):
try:
rospy.sleep(duration)
except rospy.exceptions.ROSInterruptException:
pass
def idle_loop(self):
self.last_activity_time = rospy.get_time()
while (rospy.get_time() - self.last_activity_time < 10 or
len(self.builtinsounds) + len(self.voicesounds) + len(self.filesounds) > 0) \
and not rospy.is_shutdown():
#print "idle_loop"
self.diagnostics(0)
self.sleep(1)
self.cleanup()
