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 import os
38 import sys
39 import threading
40 import traceback
41 import yaml
42 
43 import actionlib
44 import roslib
45 import rospkg
46 import rospy
47 
48 from diagnostic_msgs.msg import DiagnosticArray
49 from diagnostic_msgs.msg import DiagnosticStatus
50 from diagnostic_msgs.msg import KeyValue
51 from sound_play.msg import SoundRequest
52 from sound_play.msg import SoundRequestAction
53 from sound_play.msg import SoundRequestFeedback
54 from sound_play.msg import SoundRequestResult
55 from sound_play.sound_type import SoundType
56 
57 try:
58  import gi
59  gi.require_version('Gst', '1.0')
60  from gi.repository import GObject as GObject
61  from gi.repository import Gst as Gst
62 except Exception:
63  str = """
64 **************************************************************
65 Error opening pygst. Is gstreamer installed?
66 **************************************************************
67 """
68  rospy.logfatal(str)
69  # print str
70  exit(1)
71 
72 
73 class SoundPlayNode(object):
74  _feedback = SoundRequestFeedback()
75  _result = SoundRequestResult()
76 
77  def stopdict(self, dict):
78  for sound in dict.values():
79  sound.stop()
80 
81  def stopall(self):
82  self.stopdict(self.builtinsounds)
83  self.stopdict(self.filesounds)
84  self.stopdict(self.voicesounds)
85 
86  def select_sound(self, data):
87  if data.sound == SoundRequest.PLAY_FILE:
88  if not data.arg2:
89  if data.arg not in self.filesounds.keys():
90  rospy.logdebug(
91  'command for uncached wave: "%s"' % data.arg)
92  try:
93  self.filesounds[data.arg] = SoundType(
94  data.arg, self.device, data.volume)
95  except Exception:
96  rospy.logerr(
97  'Error setting up to play "%s".'
98  'Does this file exist on the machine'
99  'on which sound_play is running?' % data.arg)
100  return
101  else:
102  rospy.logdebug('command for cached wave: "%s"' % data.arg)
103  filesound = self.filesounds[data.arg]
104  if filesound.sound.get_property('volume') != data.volume:
105  rospy.logdebug(
106  'volume for cached wave has changed,'
107  'resetting volume')
108  filesound.sound.set_property('volume', data.volume)
109  sound = self.filesounds[data.arg]
110  else:
111  absfilename = os.path.join(
112  roslib.packages.get_pkg_dir(data.arg2), data.arg)
113  if absfilename not in self.filesounds.keys():
114  rospy.logdebug(
115  'command for uncached wave: "%s"' % absfilename)
116  try:
117  self.filesounds[absfilename] = SoundType(
118  absfilename, self.device, data.volume)
119  except Exception:
120  rospy.logerr(
121  'Error setting up to play "%s" from package "%s".'
122  'Does this file exist on the machine '
123  'on which sound_play is running?'
124  % (data.arg, data.arg2))
125  return
126  else:
127  rospy.logdebug(
128  'command for cached wave: "%s"' % absfilename)
129  filesound = self.filesounds[absfilename]
130  if filesound.sound.get_property('volume') != data.volume:
131  rospy.logdebug(
132  'volume for cached wave has changed,'
133  'resetting volume')
134  filesound.sound.set_property('volume', data.volume)
135  sound = self.filesounds[absfilename]
136  elif data.sound == SoundRequest.SAY:
137  voice_key = data.arg + '---' + data.arg2
138  if voice_key not in self.voicesounds.keys():
139  rospy.logdebug('command for uncached text: "%s"' % voice_key)
140  if self.plugin is None:
141  rospy.logerr(
142  'Plugin is not found {}.'.format(self.plugin_name))
143  else:
144  if data.arg2 == '':
145  voice = self.default_voice
146  else:
147  voice = data.arg2
148  wavfilename = self.plugin.sound_play_say_plugin(
149  data.arg, voice)
150  if wavfilename is None:
151  rospy.logerr('Failed to generate wavfile.')
152  else:
153  self.voicesounds[voice_key] = SoundType(
154  wavfilename, self.device, data.volume)
155  else:
156  rospy.logdebug('command for cached text: "%s"' % voice_key)
157  voicesound = self.voicesounds[voice_key]
158  if voicesound.sound.get_property('volume') != data.volume:
159  rospy.logdebug(
160  'volume for cached text has changed, resetting volume')
161  voicesound.sound.set_property('volume', data.volume)
162  sound = self.voicesounds[voice_key]
163  else:
164  rospy.logdebug('command for builtin wave: %i' % data.sound)
165  if ((data.sound in self.builtinsounds and
166  data.volume != self.builtinsounds[data.sound].volume)
167  or data.sound not in self.builtinsounds):
168  params = self.builtinsoundparams[data.sound]
169  volume = data.volume
170  # use the second param as a scaling for the input volume
171  if params[1] != 1:
172  volume = (volume + params[1])/2
173  self.builtinsounds[data.sound] = SoundType(
174  params[0], self.device, volume)
175  sound = self.builtinsounds[data.sound]
176  if sound.staleness != 0 and data.command != SoundRequest.PLAY_STOP:
177  # This sound isn't counted in active_sounds
178  rospy.logdebug("activating %i %s" % (data.sound, data.arg))
179  self.active_sounds = self.active_sounds + 1
180  sound.staleness = 0
181  return sound
182 
183  def callback(self, data):
184  if not self.initialized:
185  return
186  self.mutex.acquire()
187  try:
188  if (data.sound == SoundRequest.ALL
189  and data.command == SoundRequest.PLAY_STOP):
190  self.stopall()
191  else:
192  sound = self.select_sound(data)
193  sound.command(data.command)
194  except Exception as e:
195  rospy.logerr('Exception in callback: %s' % str(e))
196  rospy.loginfo(traceback.format_exc())
197  finally:
198  self.mutex.release()
199  rospy.logdebug("done callback")
200 
201  # Purge sounds that haven't been played in a while.
202  def cleanupdict(self, dict):
203  purgelist = []
204  for key, sound in iter(dict.items()):
205  try:
206  staleness = sound.get_staleness()
207  except Exception as e:
208  rospy.logerr(
209  'Exception in cleanupdict for sound (%s): %s'
210  % (str(key), str(e)))
211  # Something is wrong. Let's purge and try again.
212  staleness = 100
213  # print "%s %i"%(key, staleness)
214  if staleness >= 10:
215  purgelist.append(key)
216  # Sound is playing
217  if staleness == 0:
218  self.active_sounds = self.active_sounds + 1
219  for key in purgelist:
220  rospy.logdebug('Purging %s from cache' % key)
221  # clean up resources
222  dict[key].dispose()
223  del dict[key]
224 
225  def cleanup(self):
226  self.mutex.acquire()
227  try:
228  self.active_sounds = 0
229  self.cleanupdict(self.filesounds)
230  self.cleanupdict(self.voicesounds)
231  self.cleanupdict(self.builtinsounds)
232  except Exception:
233  rospy.loginfo(
234  'Exception in cleanup: %s' % sys.exc_info()[0])
235  finally:
236  self.mutex.release()
237 
238  def diagnostics(self, state):
239  try:
240  da = DiagnosticArray()
241  ds = DiagnosticStatus()
242  ds.name = rospy.get_caller_id().lstrip('/') + ": Node State"
243  if state == 0:
244  ds.level = DiagnosticStatus.OK
245  ds.message = "%i sounds playing" % self.active_sounds
246  ds.values.append(
247  KeyValue("Active sounds", str(self.active_sounds)))
248  ds.values.append(
249  KeyValue(
250  "Allocated sound channels",
251  str(self.num_channels)))
252  ds.values.append(
253  KeyValue(
254  "Buffered builtin sounds",
255  str(len(self.builtinsounds))))
256  ds.values.append(
257  KeyValue(
258  "Buffered wave sounds",
259  str(len(self.filesounds))))
260  ds.values.append(
261  KeyValue(
262  "Buffered voice sounds",
263  str(len(self.voicesounds))))
264  elif state == 1:
265  ds.level = DiagnosticStatus.WARN
266  ds.message = "Sound device not open yet."
267  else:
268  ds.level = DiagnosticStatus.ERROR
269  ds.message = "Can't open sound device." +\
270  "See http://wiki.ros.org/sound_play/Troubleshooting"
271  da.status.append(ds)
272  da.header.stamp = rospy.get_rostime()
273  self.diagnostic_pub.publish(da)
274  except Exception as e:
275  rospy.loginfo('Exception in diagnostics: %s' % str(e))
276 
277  def execute_cb(self, data):
278  data = data.sound_request
279  if not self.initialized:
280  rospy.logerr('soundplay_node is not initialized yet.')
281  self._as.set_aborted()
282  return
283  self.mutex.acquire()
284  # Force only one sound at a time
285  self.stopall()
286  try:
287  if (data.sound == SoundRequest.ALL
288  and data.command == SoundRequest.PLAY_STOP):
289  self.stopall()
290  else:
291  sound = self.select_sound(data)
292  sound.command(data.command)
293 
294  r = rospy.Rate(self.loop_rate)
295  start_time = rospy.get_rostime()
296  success = True
297  while sound.get_playing():
298  sound.update()
299  if self._as.is_preempt_requested():
300  rospy.loginfo('sound_play action: Preempted')
301  sound.stop()
302  self._as.set_preempted()
303  success = False
304  break
305 
306  self._feedback.playing = sound.get_playing()
307  self._feedback.stamp = rospy.get_rostime() - start_time
308  self._as.publish_feedback(self._feedback)
309  r.sleep()
310 
311  if success:
312  self._result.playing = self._feedback.playing
313  self._result.stamp = self._feedback.stamp
314  rospy.loginfo('sound_play action: Succeeded')
315  self._as.set_succeeded(self._result)
316 
317  except Exception as e:
318  self._as.set_aborted()
319  rospy.logerr(
320  'Exception in actionlib callback: %s' % str(e))
321  rospy.loginfo(traceback.format_exc())
322  finally:
323  self.mutex.release()
324  rospy.logdebug("done actionlib callback")
325 
326  def __init__(self):
327  Gst.init(None)
328 
329  # Start gobject thread to receive gstreamer messages
330  GObject.threads_init()
331  self.g_loop = threading.Thread(target=GObject.MainLoop().run)
332  self.g_loop.daemon = True
333  self.g_loop.start()
334 
335  rospy.init_node('sound_play')
336  self.loop_rate = rospy.get_param('~loop_rate', 100)
337  self.device = rospy.get_param("~device", "default")
338  self.default_voice = rospy.get_param('~default_voice', None)
339  self.plugin_name = rospy.get_param(
340  '~plugin', 'sound_play/festival_plugin')
341  self.diagnostic_pub = rospy.Publisher(
342  "/diagnostics", DiagnosticArray, queue_size=1)
343  rootdir = os.path.join(
344  roslib.packages.get_pkg_dir('sound_play'), 'sounds')
345 
346  # load plugin
347  rospack = rospkg.RosPack()
348  depend_pkgs = rospack.get_depends_on('sound_play', implicit=False)
349  depend_pkgs = ['sound_play'] + depend_pkgs
350  rospy.loginfo("Loading from plugin definitions")
351  plugin_yamls = []
352  for depend_pkg in depend_pkgs:
353  manifest = rospack.get_manifest(depend_pkg)
354  plugin_yaml = manifest.get_export('sound_play', 'plugin')
355  if len(plugin_yaml) != 0:
356  plugin_yamls += plugin_yaml
357  for plugin_y in plugin_yaml:
358  rospy.logdebug('Loading plugin in {}'.format(plugin_y))
359  plugin_dict = {}
360  for plugin_yaml in plugin_yamls:
361  if not os.path.exists(plugin_yaml):
362  rospy.logerr(
363  'Failed to load plugin yaml: {}'.format(plugin_yaml))
364  rospy.logerr(
365  'Missing plugin yaml: {}'.format(plugin_yaml))
366  continue
367  with open(plugin_yaml) as f:
368  plugin_descs = yaml.safe_load(f)
369  for plugin_desc in plugin_descs:
370  plugin_dict[plugin_desc['name']] = plugin_desc['module']
371 
372  self.plugin = None
373  if self.plugin_name in plugin_dict.keys():
374  plugin_module = plugin_dict[self.plugin_name]
375  mod = __import__(plugin_module.split('.')[0])
376  for sub_mod in plugin_module.split('.')[1:]:
377  mod = getattr(mod, sub_mod)
378  self.plugin = mod()
379 
381  SoundRequest.BACKINGUP: (
382  os.path.join(rootdir, 'BACKINGUP.ogg'), 0.1),
383  SoundRequest.NEEDS_UNPLUGGING: (
384  os.path.join(rootdir, 'NEEDS_UNPLUGGING.ogg'), 1),
385  SoundRequest.NEEDS_PLUGGING: (
386  os.path.join(rootdir, 'NEEDS_PLUGGING.ogg'), 1),
387  SoundRequest.NEEDS_UNPLUGGING_BADLY: (
388  os.path.join(rootdir, 'NEEDS_UNPLUGGING_BADLY.ogg'), 1),
389  SoundRequest.NEEDS_PLUGGING_BADLY: (
390  os.path.join(rootdir, 'NEEDS_PLUGGING_BADLY.ogg'), 1),
391  }
392 
393  self.no_error = True
394  self.initialized = False
395  self.active_sounds = 0
396 
397  self.mutex = threading.Lock()
398  self.sub = rospy.Subscriber("robotsound", SoundRequest, self.callback)
400  'sound_play', SoundRequestAction,
401  execute_cb=self.execute_cb, auto_start=False)
402 
403  self.mutex.acquire()
404  # For ros startup race condition
405  self.sleep(0.5)
406  self.diagnostics(1)
407 
408  while not rospy.is_shutdown():
409  while not rospy.is_shutdown():
410  self.init_vars()
411  self.no_error = True
412  self.initialized = True
413  self.mutex.release()
414  if not self._as.action_server.started:
415  self._as.start()
416  try:
417  self.idle_loop()
418  # Returns after inactive period to test device availability
419  # print "Exiting idle"
420  except Exception:
421  rospy.loginfo(
422  'Exception in idle_loop: %s' % sys.exc_info()[0])
423  finally:
424  self.mutex.acquire()
425 
426  self.diagnostics(2)
427  self.mutex.release()
428 
429  def init_vars(self):
430  self.num_channels = 10
431  self.builtinsounds = {}
432  self.filesounds = {}
433  self.voicesounds = {}
434  self.hotlist = []
435  if not self.initialized:
436  rospy.loginfo('sound_play node is ready to play sound')
437 
438  def sleep(self, duration):
439  try:
440  rospy.sleep(duration)
441  except rospy.exceptions.ROSInterruptException:
442  pass
443 
444  def get_sound_length(self):
445  sound_length = len(self.builtinsounds) +\
446  len(self.voicesounds) + len(self.filesounds)
447  return sound_length
448 
449  def idle_loop(self):
450  self.last_activity_time = rospy.get_time()
451  while (not rospy.is_shutdown()
452  and (rospy.get_time() - self.last_activity_time < 10
453  or self.get_sound_length() > 0)):
454  # print("idle_loop")
455  self.diagnostics(0)
456  self.sleep(1)
457  self.cleanup()
458  # print("idle_exiting")
459 
460 
461 if __name__ == '__main__':
462  SoundPlayNode()
def start(self, sound, volume=1.0, kwargs)
Play a buildin sound repeatedly.
def sleep(self, duration)


sound_play
Author(s): Blaise Gassend
autogenerated on Fri Jun 9 2023 02:47:15