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  if hasattr(data.arg, 'decode'):
258  txtfile.write(data.arg.decode('UTF-8').encode('ISO-8859-15'))
259  else:
260  txtfile.write(data.arg.encode('ISO-8859-15'))
261  except UnicodeEncodeError:
262  if hasattr(data.arg, 'decode'):
263  txtfile.write(data.arg)
264  else:
265  txtfile.write(data.arg.encode('UTF-8'))
266  txtfile.flush()
267  os.system("text2wave -eval '("+voice+")' "+txtfilename+" -o "+wavfilename)
268  try:
269  if os.stat(wavfilename).st_size == 0:
270  raise OSError # So we hit the same catch block
271  except OSError:
272  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')
273  return
274  self.voicesounds[data.arg] = soundtype(wavfilename, self.device, data.volume)
275  finally:
276  txtfile.close()
277  else:
278  rospy.logdebug('command for cached text: "%s"'%data.arg)
279  if self.voicesounds[data.arg].sound.get_property('volume') != data.volume:
280  rospy.logdebug('volume for cached text has changed, resetting volume')
281  self.voicesounds[data.arg].sound.set_property('volume', data.volume)
282  sound = self.voicesounds[data.arg]
283  else:
284  rospy.logdebug('command for builtin wave: %i'%data.sound)
285  if data.sound not in self.builtinsounds or (data.sound in self.builtinsounds and data.volume != self.builtinsounds[data.sound].volume):
286  params = self.builtinsoundparams[data.sound]
287  volume = data.volume
288  if params[1] != 1: # use the second param as a scaling for the input volume
289  volume = (volume + params[1])/2
290  self.builtinsounds[data.sound] = soundtype(params[0], self.device, volume)
291  sound = self.builtinsounds[data.sound]
292  if sound.staleness != 0 and data.command != SoundRequest.PLAY_STOP:
293  # This sound isn't counted in active_sounds
294  rospy.logdebug("activating %i %s"%(data.sound,data.arg))
295  self.active_sounds = self.active_sounds + 1
296  sound.staleness = 0
297  # if self.active_sounds > self.num_channels:
298  # mixer.set_num_channels(self.active_sounds)
299  # self.num_channels = self.active_sounds
300  return sound
301 
302  def callback(self,data):
303  if not self.initialized:
304  return
305  self.mutex.acquire()
306  # Force only one sound at a time
307  self.stopall()
308  try:
309  if data.sound == SoundRequest.ALL and data.command == SoundRequest.PLAY_STOP:
310  self.stopall()
311  else:
312  sound = self.select_sound(data)
313  sound.command(data.command)
314  except Exception as e:
315  rospy.logerr('Exception in callback: %s'%str(e))
316  rospy.loginfo(traceback.format_exc())
317  finally:
318  self.mutex.release()
319  rospy.logdebug("done callback")
320 
321  # Purge sounds that haven't been played in a while.
322  def cleanupdict(self, dict):
323  purgelist = []
324  for (key,sound) in iter(dict.items()):
325  try:
326  staleness = sound.get_staleness()
327  except Exception as e:
328  rospy.logerr('Exception in cleanupdict for sound (%s): %s'%(str(key),str(e)))
329  staleness = 100 # Something is wrong. Let's purge and try again.
330  #print "%s %i"%(key, staleness)
331  if staleness >= 10:
332  purgelist.append(key)
333  if staleness == 0: # Sound is playing
334  self.active_sounds = self.active_sounds + 1
335  for key in purgelist:
336  rospy.logdebug('Purging %s from cache'%key)
337  dict[key].dispose() # clean up resources
338  del dict[key]
339 
340  def cleanup(self):
341  self.mutex.acquire()
342  try:
343  self.active_sounds = 0
344  self.cleanupdict(self.filesounds)
345  self.cleanupdict(self.voicesounds)
346  self.cleanupdict(self.builtinsounds)
347  except:
348  rospy.loginfo('Exception in cleanup: %s'%sys.exc_info()[0])
349  finally:
350  self.mutex.release()
351 
352  def diagnostics(self, state):
353  try:
354  da = DiagnosticArray()
355  ds = DiagnosticStatus()
356  ds.name = rospy.get_caller_id().lstrip('/') + ": Node State"
357  if state == 0:
358  ds.level = DiagnosticStatus.OK
359  ds.message = "%i sounds playing"%self.active_sounds
360  ds.values.append(KeyValue("Active sounds", str(self.active_sounds)))
361  ds.values.append(KeyValue("Allocated sound channels", str(self.num_channels)))
362  ds.values.append(KeyValue("Buffered builtin sounds", str(len(self.builtinsounds))))
363  ds.values.append(KeyValue("Buffered wave sounds", str(len(self.filesounds))))
364  ds.values.append(KeyValue("Buffered voice sounds", str(len(self.voicesounds))))
365  elif state == 1:
366  ds.level = DiagnosticStatus.WARN
367  ds.message = "Sound device not open yet."
368  else:
369  ds.level = DiagnosticStatus.ERROR
370  ds.message = "Can't open sound device. See http://wiki.ros.org/sound_play/Troubleshooting"
371  da.status.append(ds)
372  da.header.stamp = rospy.get_rostime()
373  self.diagnostic_pub.publish(da)
374  except Exception as e:
375  rospy.loginfo('Exception in diagnostics: %s'%str(e))
376 
377  def execute_cb(self, data):
378  data = data.sound_request
379  if not self.initialized:
380  return
381  self.mutex.acquire()
382  # Force only one sound at a time
383  self.stopall()
384  try:
385  if data.sound == SoundRequest.ALL and data.command == SoundRequest.PLAY_STOP:
386  self.stopall()
387  else:
388  sound = self.select_sound(data)
389  sound.command(data.command)
390 
391  r = rospy.Rate(1)
392  start_time = rospy.get_rostime()
393  success = True
394  while sound.get_playing():
395  sound.update()
396  if self._as.is_preempt_requested():
397  rospy.loginfo('sound_play action: Preempted')
398  sound.stop()
399  self._as.set_preempted()
400  success = False
401  break
402 
403  self._feedback.playing = sound.get_playing()
404  self._feedback.stamp = rospy.get_rostime() - start_time
405  self._as.publish_feedback(self._feedback)
406  r.sleep()
407 
408  if success:
409  self._result.playing = self._feedback.playing
410  self._result.stamp = self._feedback.stamp
411  rospy.loginfo('sound_play action: Succeeded')
412  self._as.set_succeeded(self._result)
413 
414  except Exception as e:
415  rospy.logerr('Exception in actionlib callback: %s'%str(e))
416  rospy.loginfo(traceback.format_exc())
417  finally:
418  self.mutex.release()
419  rospy.logdebug("done actionlib callback")
420 
421  def __init__(self):
422  Gst.init(None)
423  rospy.init_node('sound_play')
424  self.device = rospy.get_param("~device", "default")
425  self.diagnostic_pub = rospy.Publisher("/diagnostics", DiagnosticArray, queue_size=1)
426  rootdir = os.path.join(roslib.packages.get_pkg_dir('sound_play'),'sounds')
427 
429  SoundRequest.BACKINGUP : (os.path.join(rootdir, 'BACKINGUP.ogg'), 0.1),
430  SoundRequest.NEEDS_UNPLUGGING : (os.path.join(rootdir, 'NEEDS_UNPLUGGING.ogg'), 1),
431  SoundRequest.NEEDS_PLUGGING : (os.path.join(rootdir, 'NEEDS_PLUGGING.ogg'), 1),
432  SoundRequest.NEEDS_UNPLUGGING_BADLY : (os.path.join(rootdir, 'NEEDS_UNPLUGGING_BADLY.ogg'), 1),
433  SoundRequest.NEEDS_PLUGGING_BADLY : (os.path.join(rootdir, 'NEEDS_PLUGGING_BADLY.ogg'), 1),
434  }
435 
436  self.no_error = True
437  self.initialized = False
438  self.active_sounds = 0
439 
440  self.mutex = threading.Lock()
441  sub = rospy.Subscriber("robotsound", SoundRequest, self.callback)
442  self._as = actionlib.SimpleActionServer('sound_play', SoundRequestAction, execute_cb=self.execute_cb, auto_start = False)
443  self._as.start()
444 
445  self.mutex.acquire()
446  self.sleep(0.5) # For ros startup race condition
447  self.diagnostics(1)
448 
449  while not rospy.is_shutdown():
450  while not rospy.is_shutdown():
451  self.init_vars()
452  self.no_error = True
453  self.initialized = True
454  self.mutex.release()
455  try:
456  self.idle_loop()
457  # Returns after inactive period to test device availability
458  #print "Exiting idle"
459  except:
460  rospy.loginfo('Exception in idle_loop: %s'%sys.exc_info()[0])
461  finally:
462  self.mutex.acquire()
463 
464  self.diagnostics(2)
465  self.mutex.release()
466 
467  def init_vars(self):
468  self.num_channels = 10
469  self.builtinsounds = {}
470  self.filesounds = {}
471  self.voicesounds = {}
472  self.hotlist = []
473  if not self.initialized:
474  rospy.loginfo('sound_play node is ready to play sound')
475 
476  def sleep(self, duration):
477  try:
478  rospy.sleep(duration)
479  except rospy.exceptions.ROSInterruptException:
480  pass
481 
482  def idle_loop(self):
483  self.last_activity_time = rospy.get_time()
484  while (rospy.get_time() - self.last_activity_time < 10 or
485  len(self.builtinsounds) + len(self.voicesounds) + len(self.filesounds) > 0) \
486  and not rospy.is_shutdown():
487  #print "idle_loop"
488  self.diagnostics(0)
489  self.sleep(1)
490  self.cleanup()
491  #print "idle_exiting"
492 
493 if __name__ == '__main__':
494  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 Apr 9 2021 02:41:17