00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 import roslib; roslib.load_manifest('sound_play')
00040
00041 import rospy
00042 import threading
00043 from sound_play.msg import SoundRequest
00044 import os
00045 import logging
00046 import sys
00047 import traceback
00048 import tempfile
00049 from diagnostic_msgs.msg import DiagnosticStatus, KeyValue, DiagnosticArray
00050
00051 try:
00052 import pygst
00053 pygst.require('0.10')
00054 import gst
00055 import gobject
00056 except:
00057 str="""
00058 **************************************************************
00059 Error opening pygst. Is gstreamer installed? (sudo apt-get install python-gst0.10
00060 **************************************************************
00061 """
00062 rospy.logfatal(str)
00063 print str
00064 exit(1)
00065
00066 def sleep(t):
00067 try:
00068 rospy.sleep(t)
00069 except:
00070 pass
00071
00072
00073 class soundtype:
00074 STOPPED = 0
00075 LOOPING = 1
00076 COUNTING = 2
00077
00078 def __init__(self, file, volume = 1.0):
00079 self.lock = threading.RLock()
00080 self.state = self.STOPPED
00081 self.sound = gst.element_factory_make("playbin","player")
00082 if (":" in file):
00083 uri = file
00084 elif os.path.isfile(file):
00085 uri = "file://" + os.path.abspath(file)
00086 else:
00087 rospy.logerr('Error: URI is invalid: %s'%file)
00088
00089 self.uri = uri
00090 self.volume = volume
00091 self.sound.set_property('uri', uri)
00092 self.sound.set_property("volume",volume)
00093 self.staleness = 1
00094 self.file = file
00095
00096 def __del__(self):
00097
00098 self.stop()
00099
00100 def loop(self):
00101 self.lock.acquire()
00102 try:
00103 self.staleness = 0
00104 if self.state == self.COUNTING:
00105 self.stop()
00106
00107 if self.state == self.STOPPED:
00108 self.sound.seek_simple(gst.FORMAT_TIME, gst.SEEK_FLAG_FLUSH, 0)
00109 self.sound.set_state(gst.STATE_PLAYING)
00110
00111 self.state = self.LOOPING
00112 finally:
00113 self.lock.release()
00114
00115 def stop(self):
00116 if self.state != self.STOPPED:
00117 self.lock.acquire()
00118 try:
00119 self.sound.set_state(gst.STATE_NULL)
00120 self.state = self.STOPPED
00121 finally:
00122 self.lock.release()
00123
00124 def single(self):
00125 self.lock.acquire()
00126 try:
00127 rospy.logdebug("Playing %s"%self.uri)
00128 self.staleness = 0
00129 if self.state == self.LOOPING:
00130 self.stop()
00131
00132 self.sound.seek_simple(gst.FORMAT_TIME, gst.SEEK_FLAG_FLUSH, 0)
00133 self.sound.set_state(gst.STATE_PLAYING)
00134
00135 self.state = self.COUNTING
00136 finally:
00137 self.lock.release()
00138
00139 def command(self, cmd):
00140 if cmd == SoundRequest.PLAY_STOP:
00141 self.stop()
00142 elif cmd == SoundRequest.PLAY_ONCE:
00143 self.single()
00144 elif cmd == SoundRequest.PLAY_START:
00145 self.loop()
00146
00147 def get_staleness(self):
00148 self.lock.acquire()
00149 position = 0
00150 duration = 0
00151 try:
00152 position = self.sound.query_position(gst.FORMAT_TIME)[0]
00153 duration = self.sound.query_duration(gst.FORMAT_TIME)[0]
00154 except Exception, e:
00155 position = 0
00156 duration = 0
00157 finally:
00158 self.lock.release()
00159
00160 if position != duration:
00161 self.staleness = 0
00162 else:
00163 self.staleness = self.staleness + 1
00164 return self.staleness
00165
00166 class soundplay:
00167 def stopdict(self,dict):
00168 for sound in dict.values():
00169 sound.stop()
00170
00171 def stopall(self):
00172 self.stopdict(self.builtinsounds)
00173 self.stopdict(self.filesounds)
00174 self.stopdict(self.voicesounds)
00175
00176 def callback(self,data):
00177 if not self.initialized:
00178 return
00179 self.mutex.acquire()
00180
00181
00182 self.stopall()
00183 try:
00184 if data.sound == SoundRequest.ALL and data.command == SoundRequest.PLAY_STOP:
00185 self.stopall()
00186 else:
00187 if data.sound == SoundRequest.PLAY_FILE:
00188 if not data.arg in self.filesounds.keys():
00189 rospy.logdebug('command for uncached wave: "%s"'%data.arg)
00190 try:
00191 self.filesounds[data.arg] = soundtype(data.arg)
00192 except:
00193 print "Exception"
00194 rospy.logerr('Error setting up to play "%s". Does this file exist on the machine on which sound_play is running?'%data.arg)
00195 return
00196 else:
00197 print "cached"
00198 rospy.logdebug('command for cached wave: "%s"'%data.arg)
00199 sound = self.filesounds[data.arg]
00200 elif data.sound == SoundRequest.SAY:
00201 if not data.arg in self.voicesounds.keys():
00202 rospy.logdebug('command for uncached text: "%s"' % data.arg)
00203 txtfile = tempfile.NamedTemporaryFile(prefix='sound_play', suffix='.txt')
00204 (wavfile,wavfilename) = tempfile.mkstemp(prefix='sound_play', suffix='.wav')
00205 txtfilename=txtfile.name
00206 os.close(wavfile)
00207 voice = data.arg2
00208 try:
00209 txtfile.write(data.arg)
00210 txtfile.flush()
00211 os.system("text2wave -eval '("+voice+")' "+txtfilename+" -o "+wavfilename)
00212 try:
00213 if os.stat(wavfilename).st_size == 0:
00214 raise OSError
00215 except OSError:
00216 rospy.logerr('Sound synthesis failed. Is festival installed? Is a festival voice installed? Try running "rosdep satisfy sound_play|sh". Refer to http://pr.willowgarage.com/wiki/sound_play/Troubleshooting')
00217 return
00218 self.voicesounds[data.arg] = soundtype(wavfilename)
00219 finally:
00220 txtfile.close()
00221 else:
00222 rospy.logdebug('command for cached text: "%s"'%data.arg)
00223 sound = self.voicesounds[data.arg]
00224 else:
00225 rospy.logdebug('command for builtin wave: %i'%data.sound)
00226 if not data.sound in self.builtinsounds:
00227 params = self.builtinsoundparams[data.sound]
00228 self.builtinsounds[data.sound] = soundtype(params[0], params[1])
00229 sound = self.builtinsounds[data.sound]
00230 if sound.staleness != 0 and data.command != SoundRequest.PLAY_STOP:
00231
00232 rospy.logdebug("activating %i %s"%(data.sound,data.arg))
00233 self.active_sounds = self.active_sounds + 1
00234 sound.staleness = 0
00235
00236
00237
00238 sound.command(data.command)
00239 except Exception, e:
00240 rospy.logerr('Exception in callback: %s'%str(e))
00241 rospy.loginfo(traceback.format_exc())
00242 finally:
00243 self.mutex.release()
00244 rospy.logdebug("done callback")
00245
00246
00247 def cleanupdict(self, dict):
00248 purgelist = []
00249 for (key,sound) in dict.iteritems():
00250 try:
00251 staleness = sound.get_staleness()
00252 except Exception, e:
00253 rospy.logerr('Exception in cleanupdict for sound (%s): %s'%(str(key),str(e)))
00254 staleness = 100
00255
00256 if staleness >= 10:
00257 purgelist.append(key)
00258 if staleness == 0:
00259 self.active_sounds = self.active_sounds + 1
00260 for key in purgelist:
00261 rospy.logdebug('Purging %s from cache'%key)
00262 del dict[key]
00263
00264 def cleanup(self):
00265 self.mutex.acquire()
00266 try:
00267 self.active_sounds = 0
00268 self.cleanupdict(self.filesounds)
00269 self.cleanupdict(self.voicesounds)
00270 self.cleanupdict(self.builtinsounds)
00271 except:
00272 rospy.loginfo('Exception in cleanup: %s'%sys.exc_info()[0])
00273 finally:
00274 self.mutex.release()
00275
00276 def diagnostics(self, state):
00277 try:
00278 da = DiagnosticArray()
00279 ds = DiagnosticStatus()
00280 ds.name = rospy.get_caller_id().lstrip('/') + ": Node State"
00281 if state == 0:
00282 ds.level = DiagnosticStatus.OK
00283 ds.message = "%i sounds playing"%self.active_sounds
00284 ds.values.append(KeyValue("Active sounds", str(self.active_sounds)))
00285 ds.values.append(KeyValue("Allocated sound channels", str(self.num_channels)))
00286 ds.values.append(KeyValue("Buffered builtin sounds", str(len(self.builtinsounds))))
00287 ds.values.append(KeyValue("Buffered wave sounds", str(len(self.filesounds))))
00288 ds.values.append(KeyValue("Buffered voice sounds", str(len(self.voicesounds))))
00289 elif state == 1:
00290 ds.level = DiagnosticStatus.WARN
00291 ds.message = "Sound device not open yet."
00292 else:
00293 ds.level = DiagnosticStatus.ERROR
00294 ds.message = "Can't open sound device. See http://pr.willowgarage.com/wiki/sound_play/Troubleshooting"
00295 da.status.append(ds)
00296 da.header.stamp = rospy.get_rostime()
00297 self.diagnostic_pub.publish(da)
00298 except Exception, e:
00299 rospy.loginfo('Exception in diagnostics: %s'%str(e))
00300
00301 def __init__(self):
00302 rospy.init_node('sound_play')
00303 self.diagnostic_pub = rospy.Publisher("/diagnostics", DiagnosticArray)
00304
00305 rootdir = os.path.join(os.path.dirname(__file__),'..','sounds')
00306
00307 self.builtinsoundparams = {
00308 SoundRequest.BACKINGUP : (os.path.join(rootdir, 'BACKINGUP.ogg'), 0.1),
00309 SoundRequest.NEEDS_UNPLUGGING : (os.path.join(rootdir, 'NEEDS_UNPLUGGING.ogg'), 1),
00310 SoundRequest.NEEDS_PLUGGING : (os.path.join(rootdir, 'NEEDS_PLUGGING.ogg'), 1),
00311 SoundRequest.NEEDS_UNPLUGGING_BADLY : (os.path.join(rootdir, 'NEEDS_UNPLUGGING_BADLY.ogg'), 1),
00312 SoundRequest.NEEDS_PLUGGING_BADLY : (os.path.join(rootdir, 'NEEDS_PLUGGING_BADLY.ogg'), 1),
00313 }
00314
00315 self.mutex = threading.Lock()
00316 sub = rospy.Subscriber("robotsound", SoundRequest, self.callback)
00317 self.mutex.acquire()
00318 self.no_error = True
00319 self.initialized = False
00320 self.active_sounds = 0
00321 self.sleep(0.5)
00322 self.diagnostics(1)
00323
00324 while not rospy.is_shutdown():
00325 while not rospy.is_shutdown():
00326 self.init_vars()
00327 self.no_error = True
00328 self.initialized = True
00329 self.mutex.release()
00330 try:
00331 self.idle_loop()
00332
00333
00334 except:
00335 rospy.loginfo('Exception in idle_loop: %s'%sys.exc_info()[0])
00336 finally:
00337 self.mutex.acquire()
00338
00339 self.diagnostics(2)
00340 self.mutex.release()
00341
00342 def init_vars(self):
00343 self.num_channels = 10
00344 self.builtinsounds = {}
00345 self.filesounds = {}
00346 self.voicesounds = {}
00347 self.hotlist = []
00348 if not self.initialized:
00349 rospy.loginfo('sound_play node is ready to play sound')
00350
00351 def sleep(self, duration):
00352 try:
00353 rospy.sleep(duration)
00354 except rospy.exceptions.ROSInterruptException:
00355 pass
00356
00357 def idle_loop(self):
00358 self.last_activity_time = rospy.get_time()
00359 while (rospy.get_time() - self.last_activity_time < 10 or
00360 len(self.builtinsounds) + len(self.voicesounds) + len(self.filesounds) > 0) \
00361 and not rospy.is_shutdown():
00362
00363 self.diagnostics(0)
00364 self.sleep(1)
00365 self.cleanup()
00366
00367
00368 if __name__ == '__main__':
00369 soundplay()
00370
