soundplay_node.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 #***********************************************************
4 #* Software License Agreement (BSD License)
5 #*
6 #* Copyright (c) 2009, Willow Garage, Inc.
7 #* All rights reserved.
8 #*
9 #* Redistribution and use in source and binary forms, with or without
10 #* modification, are permitted provided that the following conditions
11 #* are met:
12 #*
13 #* * Redistributions of source code must retain the above copyright
14 #* notice, this list of conditions and the following disclaimer.
15 #* * Redistributions in binary form must reproduce the above
16 #* copyright notice, this list of conditions and the following
17 #* disclaimer in the documentation and/or other materials provided
18 #* with the distribution.
19 #* * Neither the name of the Willow Garage nor the names of its
20 #* contributors may be used to endorse or promote products derived
21 #* from this software without specific prior written permission.
22 #*
23 #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 #* POSSIBILITY OF SUCH DAMAGE.
35 #***********************************************************
36 
37 # Author: Blaise Gassend
38 
39 import roslib
40 import rospy
41 import threading
42 import os
43 import logging
44 import sys
45 import traceback
46 import tempfile
47 from diagnostic_msgs.msg import DiagnosticStatus, KeyValue, DiagnosticArray
48 from sound_play.msg import SoundRequest, SoundRequestAction, SoundRequestResult, SoundRequestFeedback
49 import actionlib
50 
51 try:
52  import gi
53  gi.require_version('Gst', '1.0')
54  from gi.repository import Gst as Gst
55 except:
56  str="""
57 **************************************************************
58 Error opening pygst. Is gstreamer installed?
59 **************************************************************
60 """
61  rospy.logfatal(str)
62  # print str
63  exit(1)
64 
65 def sleep(t):
66  try:
67  rospy.sleep(t)
68  except:
69  pass
70 
71 
72 class soundtype:
73  STOPPED = 0
74  LOOPING = 1
75  COUNTING = 2
76 
77  def __init__(self, file, device, volume = 1.0):
78  self.lock = threading.RLock()
79  self.state = self.STOPPED
80  self.sound = Gst.ElementFactory.make("playbin",None)
81  if self.sound is None:
82  raise Exception("Could not create sound player")
83 
84  if device:
85  self.sink = Gst.ElementFactory.make("alsasink", "sink")
86  self.sink.set_property("device", device)
87  self.sound.set_property("audio-sink", self.sink)
88 
89  if (":" in file):
90  uri = file
91  elif os.path.isfile(file):
92  uri = "file://" + os.path.abspath(file)
93  else:
94  rospy.logerr('Error: URI is invalid: %s'%file)
95 
96  self.uri = uri
97  self.volume = volume
98  self.sound.set_property('uri', uri)
99  self.sound.set_property("volume",volume)
100  self.staleness = 1
101  self.file = file
102 
103  self.bus = self.sound.get_bus()
104  self.bus.add_signal_watch()
105  self.bus_conn_id = self.bus.connect("message", self.on_stream_end)
106 
107  def on_stream_end(self, bus, message):
108  if message.type == Gst.MessageType.EOS:
109  self.stop()
110 
111  def __del__(self):
112  # stop our GST object so that it gets garbage-collected
113  self.dispose()
114 
115  def update(self):
116  if self.bus is not None:
117  self.bus.poll(Gst.MessageType.ERROR, 10)
118 
119  def loop(self):
120  self.lock.acquire()
121  try:
122  self.staleness = 0
123  if self.state == self.COUNTING:
124  self.stop()
125 
126  if self.state == self.STOPPED:
127  self.sound.seek_simple(Gst.Format.TIME, Gst.SeekFlags.FLUSH, 0)
128  self.sound.set_state(Gst.State.PLAYING)
129  self.state = self.LOOPING
130  finally:
131  self.lock.release()
132 
133  def dispose(self):
134  self.lock.acquire()
135  try:
136  if self.bus is not None:
137  self.sound.set_state(Gst.State.NULL)
138  self.bus.disconnect(self.bus_conn_id)
139  self.bus.remove_signal_watch()
140  self.bus = None
141  self.sound = None
142  self.sink = None
143  self.state = self.STOPPED
144  except Exception as e:
145  rospy.logerr('Exception in dispose: %s'%str(e))
146  finally:
147  self.lock.release()
148 
149  def stop(self):
150  if self.state != self.STOPPED:
151  self.lock.acquire()
152  try:
153  self.sound.set_state(Gst.State.NULL)
154  self.state = self.STOPPED
155  finally:
156  self.lock.release()
157 
158  def single(self):
159  self.lock.acquire()
160  try:
161  rospy.logdebug("Playing %s"%self.uri)
162  self.staleness = 0
163  if self.state == self.LOOPING:
164  self.stop()
165 
166  self.sound.seek_simple(Gst.Format.TIME, Gst.SeekFlags.FLUSH, 0)
167  self.sound.set_state(Gst.State.PLAYING)
168  self.state = self.COUNTING
169  finally:
170  self.lock.release()
171 
172  def command(self, cmd):
173  if cmd == SoundRequest.PLAY_STOP:
174  self.stop()
175  elif cmd == SoundRequest.PLAY_ONCE:
176  self.single()
177  elif cmd == SoundRequest.PLAY_START:
178  self.loop()
179 
180  def get_staleness(self):
181  self.lock.acquire()
182  position = 0
183  duration = 0
184  try:
185  position = self.sound.query_position(Gst.Format.TIME)[0]
186  duration = self.sound.query_duration(Gst.Format.TIME)[0]
187  except Exception as e:
188  position = 0
189  duration = 0
190  finally:
191  self.lock.release()
192 
193  if position != duration:
194  self.staleness = 0
195  else:
196  self.staleness = self.staleness + 1
197  return self.staleness
198 
199  def get_playing(self):
200  return self.state == self.COUNTING
201 
202 class soundplay:
203  _feedback = SoundRequestFeedback()
204  _result = SoundRequestResult()
205 
206  def stopdict(self,dict):
207  for sound in dict.values():
208  sound.stop()
209 
210  def stopall(self):
211  self.stopdict(self.builtinsounds)
212  self.stopdict(self.filesounds)
213  self.stopdict(self.voicesounds)
214 
215  def select_sound(self, data):
216  if data.sound == SoundRequest.PLAY_FILE:
217  if not data.arg2:
218  if not data.arg in self.filesounds.keys():
219  rospy.logdebug('command for uncached wave: "%s"'%data.arg)
220  try:
221  self.filesounds[data.arg] = soundtype(data.arg, self.device, data.volume)
222  except:
223  rospy.logerr('Error setting up to play "%s". Does this file exist on the machine on which sound_play is running?'%data.arg)
224  return
225  else:
226  rospy.logdebug('command for cached wave: "%s"'%data.arg)
227  if self.filesounds[data.arg].sound.get_property('volume') != data.volume:
228  rospy.logdebug('volume for cached wave has changed, resetting volume')
229  self.filesounds[data.arg].sound.set_property('volume', data.volume)
230  sound = self.filesounds[data.arg]
231  else:
232  absfilename = os.path.join(roslib.packages.get_pkg_dir(data.arg2), data.arg)
233  if not absfilename in self.filesounds.keys():
234  rospy.logdebug('command for uncached wave: "%s"'%absfilename)
235  try:
236  self.filesounds[absfilename] = soundtype(absfilename, self.device, data.volume)
237  except:
238  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))
239  return
240  else:
241  rospy.logdebug('command for cached wave: "%s"'%absfilename)
242  if self.filesounds[absfilename].sound.get_property('volume') != data.volume:
243  rospy.logdebug('volume for cached wave has changed, resetting volume')
244  self.filesounds[absfilename].sound.set_property('volume', data.volume)
245  sound = self.filesounds[absfilename]
246  elif data.sound == SoundRequest.SAY:
247  # print data
248  if not data.arg in self.voicesounds.keys():
249  rospy.logdebug('command for uncached text: "%s"' % data.arg)
250  txtfile = tempfile.NamedTemporaryFile(prefix='sound_play', suffix='.txt')
251  (wavfile,wavfilename) = tempfile.mkstemp(prefix='sound_play', suffix='.wav')
252  txtfilename=txtfile.name
253  os.close(wavfile)
254  voice = data.arg2
255  try:
256  try:
257  txtfile.write(data.arg.decode('UTF-8').encode('ISO-8859-15'))
258  except UnicodeEncodeError:
259  txtfile.write(data.arg)
260  txtfile.flush()
261  os.system("text2wave -eval '("+voice+")' "+txtfilename+" -o "+wavfilename)
262  try:
263  if os.stat(wavfilename).st_size == 0:
264  raise OSError # So we hit the same catch block
265  except OSError:
266  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')
267  return
268  self.voicesounds[data.arg] = soundtype(wavfilename, self.device, data.volume)
269  finally:
270  txtfile.close()
271  else:
272  rospy.logdebug('command for cached text: "%s"'%data.arg)
273  if self.voicesounds[data.arg].sound.get_property('volume') != data.volume:
274  rospy.logdebug('volume for cached text has changed, resetting volume')
275  self.voicesounds[data.arg].sound.set_property('volume', data.volume)
276  sound = self.voicesounds[data.arg]
277  else:
278  rospy.logdebug('command for builtin wave: %i'%data.sound)
279  if data.sound not in self.builtinsounds or (data.sound in self.builtinsounds and data.volume != self.builtinsounds[data.sound].volume):
280  params = self.builtinsoundparams[data.sound]
281  volume = data.volume
282  if params[1] != 1: # use the second param as a scaling for the input volume
283  volume = (volume + params[1])/2
284  self.builtinsounds[data.sound] = soundtype(params[0], self.device, volume)
285  sound = self.builtinsounds[data.sound]
286  if sound.staleness != 0 and data.command != SoundRequest.PLAY_STOP:
287  # This sound isn't counted in active_sounds
288  rospy.logdebug("activating %i %s"%(data.sound,data.arg))
289  self.active_sounds = self.active_sounds + 1
290  sound.staleness = 0
291  # if self.active_sounds > self.num_channels:
292  # mixer.set_num_channels(self.active_sounds)
293  # self.num_channels = self.active_sounds
294  return sound
295 
296  def callback(self,data):
297  if not self.initialized:
298  return
299  self.mutex.acquire()
300  # Force only one sound at a time
301  self.stopall()
302  try:
303  if data.sound == SoundRequest.ALL and data.command == SoundRequest.PLAY_STOP:
304  self.stopall()
305  else:
306  sound = self.select_sound(data)
307  sound.command(data.command)
308  except Exception as e:
309  rospy.logerr('Exception in callback: %s'%str(e))
310  rospy.loginfo(traceback.format_exc())
311  finally:
312  self.mutex.release()
313  rospy.logdebug("done callback")
314 
315  # Purge sounds that haven't been played in a while.
316  def cleanupdict(self, dict):
317  purgelist = []
318  for (key,sound) in dict.iteritems():
319  try:
320  staleness = sound.get_staleness()
321  except Exception as e:
322  rospy.logerr('Exception in cleanupdict for sound (%s): %s'%(str(key),str(e)))
323  staleness = 100 # Something is wrong. Let's purge and try again.
324  #print "%s %i"%(key, staleness)
325  if staleness >= 10:
326  purgelist.append(key)
327  if staleness == 0: # Sound is playing
328  self.active_sounds = self.active_sounds + 1
329  for key in purgelist:
330  rospy.logdebug('Purging %s from cache'%key)
331  dict[key].dispose() # clean up resources
332  del dict[key]
333 
334  def cleanup(self):
335  self.mutex.acquire()
336  try:
337  self.active_sounds = 0
338  self.cleanupdict(self.filesounds)
339  self.cleanupdict(self.voicesounds)
340  self.cleanupdict(self.builtinsounds)
341  except:
342  rospy.loginfo('Exception in cleanup: %s'%sys.exc_info()[0])
343  finally:
344  self.mutex.release()
345 
346  def diagnostics(self, state):
347  try:
348  da = DiagnosticArray()
349  ds = DiagnosticStatus()
350  ds.name = rospy.get_caller_id().lstrip('/') + ": Node State"
351  if state == 0:
352  ds.level = DiagnosticStatus.OK
353  ds.message = "%i sounds playing"%self.active_sounds
354  ds.values.append(KeyValue("Active sounds", str(self.active_sounds)))
355  ds.values.append(KeyValue("Allocated sound channels", str(self.num_channels)))
356  ds.values.append(KeyValue("Buffered builtin sounds", str(len(self.builtinsounds))))
357  ds.values.append(KeyValue("Buffered wave sounds", str(len(self.filesounds))))
358  ds.values.append(KeyValue("Buffered voice sounds", str(len(self.voicesounds))))
359  elif state == 1:
360  ds.level = DiagnosticStatus.WARN
361  ds.message = "Sound device not open yet."
362  else:
363  ds.level = DiagnosticStatus.ERROR
364  ds.message = "Can't open sound device. See http://wiki.ros.org/sound_play/Troubleshooting"
365  da.status.append(ds)
366  da.header.stamp = rospy.get_rostime()
367  self.diagnostic_pub.publish(da)
368  except Exception as e:
369  rospy.loginfo('Exception in diagnostics: %s'%str(e))
370 
371  def execute_cb(self, data):
372  data = data.sound_request
373  if not self.initialized:
374  return
375  self.mutex.acquire()
376  # Force only one sound at a time
377  self.stopall()
378  try:
379  if data.sound == SoundRequest.ALL and data.command == SoundRequest.PLAY_STOP:
380  self.stopall()
381  else:
382  sound = self.select_sound(data)
383  sound.command(data.command)
384 
385  r = rospy.Rate(1)
386  start_time = rospy.get_rostime()
387  success = True
388  while sound.get_playing():
389  sound.update()
390  if self._as.is_preempt_requested():
391  rospy.loginfo('sound_play action: Preempted')
392  sound.stop()
393  self._as.set_preempted()
394  success = False
395  break
396 
397  self._feedback.playing = sound.get_playing()
398  self._feedback.stamp = rospy.get_rostime() - start_time
399  self._as.publish_feedback(self._feedback)
400  r.sleep()
401 
402  if success:
403  self._result.playing = self._feedback.playing
404  self._result.stamp = self._feedback.stamp
405  rospy.loginfo('sound_play action: Succeeded')
406  self._as.set_succeeded(self._result)
407 
408  except Exception as e:
409  rospy.logerr('Exception in actionlib callback: %s'%str(e))
410  rospy.loginfo(traceback.format_exc())
411  finally:
412  self.mutex.release()
413  rospy.logdebug("done actionlib callback")
414 
415  def __init__(self):
416  Gst.init(None)
417  rospy.init_node('sound_play')
418  self.device = rospy.get_param("~device", "default")
419  self.diagnostic_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=1)
420  rootdir = os.path.join(roslib.packages.get_pkg_dir('sound_play'),'sounds')
421 
423  SoundRequest.BACKINGUP : (os.path.join(rootdir, 'BACKINGUP.ogg'), 0.1),
424  SoundRequest.NEEDS_UNPLUGGING : (os.path.join(rootdir, 'NEEDS_UNPLUGGING.ogg'), 1),
425  SoundRequest.NEEDS_PLUGGING : (os.path.join(rootdir, 'NEEDS_PLUGGING.ogg'), 1),
426  SoundRequest.NEEDS_UNPLUGGING_BADLY : (os.path.join(rootdir, 'NEEDS_UNPLUGGING_BADLY.ogg'), 1),
427  SoundRequest.NEEDS_PLUGGING_BADLY : (os.path.join(rootdir, 'NEEDS_PLUGGING_BADLY.ogg'), 1),
428  }
429 
430  self.no_error = True
431  self.initialized = False
432  self.active_sounds = 0
433 
434  self.mutex = threading.Lock()
435  sub = rospy.Subscriber("robotsound", SoundRequest, self.callback)
436  self._as = actionlib.SimpleActionServer('sound_play', SoundRequestAction, execute_cb=self.execute_cb, auto_start = False)
437  self._as.start()
438 
439  self.mutex.acquire()
440  self.sleep(0.5) # For ros startup race condition
441  self.diagnostics(1)
442 
443  while not rospy.is_shutdown():
444  while not rospy.is_shutdown():
445  self.init_vars()
446  self.no_error = True
447  self.initialized = True
448  self.mutex.release()
449  try:
450  self.idle_loop()
451  # Returns after inactive period to test device availability
452  #print "Exiting idle"
453  except:
454  rospy.loginfo('Exception in idle_loop: %s'%sys.exc_info()[0])
455  finally:
456  self.mutex.acquire()
457 
458  self.diagnostics(2)
459  self.mutex.release()
460 
461  def init_vars(self):
462  self.num_channels = 10
463  self.builtinsounds = {}
464  self.filesounds = {}
465  self.voicesounds = {}
466  self.hotlist = []
467  if not self.initialized:
468  rospy.loginfo('sound_play node is ready to play sound')
469 
470  def sleep(self, duration):
471  try:
472  rospy.sleep(duration)
473  except rospy.exceptions.ROSInterruptException:
474  pass
475 
476  def idle_loop(self):
477  self.last_activity_time = rospy.get_time()
478  while (rospy.get_time() - self.last_activity_time < 10 or
479  len(self.builtinsounds) + len(self.voicesounds) + len(self.filesounds) > 0) \
480  and not rospy.is_shutdown():
481  #print "idle_loop"
482  self.diagnostics(0)
483  self.sleep(1)
484  self.cleanup()
485  #print "idle_exiting"
486 
487 if __name__ == '__main__':
488  soundplay()
def __init__(self, file, device, volume=1.0)
def sleep(self, duration)
def on_stream_end(self, bus, message)
def cleanupdict(self, dict)
def diagnostics(self, state)
def execute_cb(self, data)
def select_sound(self, data)
def callback(self, data)
def stopdict(self, dict)


sound_play
Author(s): Blaise Gassend
autogenerated on Fri Jul 3 2020 03:44:52