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 pygame.mixer as mixer
00053 except:
00054 str="""
00055 **************************************************************
00056 Error opening pygame.mixer. Is pygame installed? (sudo apt-get install python-pygame)
00057 **************************************************************
00058 """
00059 rospy.logfatal(str)
00060 print str
00061
00062
00063 class soundtype:
00064 STOPPED = 0
00065 LOOPING = 1
00066 COUNTING = 2
00067
00068 def __init__(self, file, volume = 1.0):
00069 self.lock = threading.RLock()
00070 self.state = self.STOPPED
00071 self.chan = None
00072 self.sound = mixer.Sound(file)
00073 self.sound.set_volume(volume)
00074 self.staleness = 1
00075 self.file = file
00076
00077 def loop(self):
00078
00079
00080 self.lock.acquire()
00081 try:
00082 self.staleness = 0
00083 if self.state == self.COUNTING:
00084 self.stop()
00085
00086 if self.state == self.STOPPED:
00087 self.chan = self.sound.play(-1)
00088
00089 self.state = self.LOOPING
00090 finally:
00091 self.lock.release()
00092
00093
00094
00095 def stop(self):
00096
00097 if self.state != self.STOPPED:
00098
00099 self.lock.acquire()
00100
00101 try:
00102
00103 self.chan.fadeout(300)
00104
00105 self.state = self.STOPPED
00106 finally:
00107 self.lock.release()
00108
00109
00110
00111 def single(self):
00112
00113
00114 self.lock.acquire()
00115 try:
00116 self.staleness = 0
00117 if self.state == self.LOOPING:
00118 self.stop()
00119
00120 if self.state == self.STOPPED:
00121 self.chan = self.sound.play()
00122 else:
00123 self.chan.queue(self.sound)
00124
00125 self.state = self.COUNTING
00126 finally:
00127 self.lock.release()
00128
00129
00130
00131 def command(self, cmd):
00132 if cmd == SoundRequest.PLAY_STOP:
00133 self.stop()
00134 elif cmd == SoundRequest.PLAY_ONCE:
00135 self.single()
00136 elif cmd == SoundRequest.PLAY_START:
00137 self.loop()
00138
00139 def get_staleness(self):
00140
00141 self.lock.acquire()
00142 try:
00143 if self.chan.get_busy():
00144 self.staleness = 0
00145 else:
00146 self.staleness = self.staleness + 1
00147 return self.staleness
00148 finally:
00149 self.lock.release()
00150
00151
00152 class soundplay:
00153 def stopdict(self,dict):
00154 for sound in dict.values():
00155 sound.stop()
00156
00157 def stopall(self):
00158 self.stopdict(self.builtinsounds)
00159 self.stopdict(self.filesounds)
00160 self.stopdict(self.voicesounds)
00161
00162 def callback(self,data):
00163
00164 if not self.initialized:
00165 return
00166 self.mutex.acquire()
00167 try:
00168 if data.sound == SoundRequest.ALL and data.command == SoundRequest.PLAY_STOP:
00169 self.stopall()
00170 else:
00171 if data.sound == SoundRequest.PLAY_FILE:
00172 if not data.arg in self.filesounds.keys():
00173 rospy.logdebug('command for uncached wave: "%s"'%data.arg)
00174 try:
00175 self.filesounds[data.arg] = soundtype(data.arg)
00176 except:
00177 rospy.logerr('Error setting up to play "%s". Does this file exist on the machine on which sound_play is running?'%data.arg)
00178 return
00179 else:
00180 rospy.logdebug('command for cached wave: "%s"'%data.arg)
00181 sound = self.filesounds[data.arg]
00182 elif data.sound == SoundRequest.SAY:
00183 if not data.arg in self.voicesounds.keys():
00184 rospy.logdebug('command for uncached text: "%s"'%data.arg)
00185 txtfile = tempfile.NamedTemporaryFile(prefix='sound_play', suffix='txt')
00186 wavfile = tempfile.NamedTemporaryFile(prefix='sound_play', suffix='wav')
00187 txtfilename=txtfile.name
00188 wavfilename=wavfile.name
00189 try:
00190 txtfile.write(data.arg)
00191 txtfile.flush()
00192 os.system('text2wave '+txtfilename+' -o '+wavfilename)
00193 try:
00194 if os.stat(wavfilename).st_size == 0:
00195 raise OSError
00196 except OSError:
00197 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')
00198 return
00199 self.voicesounds[data.arg] = soundtype(wavfilename)
00200 finally:
00201 txtfile.close()
00202 wavfile.close()
00203 else:
00204 rospy.logdebug('command for cached text: "%s"'%data.arg)
00205 sound = self.voicesounds[data.arg]
00206 else:
00207 rospy.logdebug('command for builtin wave: %i'%data.sound)
00208 if not data.sound in self.builtinsounds:
00209 params = self.builtinsoundparams[data.sound]
00210 self.builtinsounds[data.sound] = soundtype(params[0], params[1])
00211 sound = self.builtinsounds[data.sound]
00212 if sound.staleness != 0 and data.command != SoundRequest.PLAY_STOP:
00213
00214
00215 self.active_sounds = self.active_sounds + 1
00216 sound.staleness = 0
00217
00218
00219
00220 sound.command(data.command)
00221 except Exception, e:
00222 rospy.logerr('Exception in callback: %s'%str(e))
00223 rospy.loginfo(traceback.format_exc())
00224 finally:
00225 self.mutex.release()
00226
00227
00228
00229 def cleanupdict(self, dict):
00230 purgelist = []
00231 for (key,sound) in dict.iteritems():
00232 try:
00233 staleness = sound.get_staleness()
00234 except Exception, e:
00235 rospy.logerr('Exception in cleanupdict for sound (%s): %s'%(str(key),str(e)))
00236 staleness = 100
00237
00238 if staleness >= 10:
00239 purgelist.append(key)
00240 if staleness == 0:
00241 self.active_sounds = self.active_sounds + 1
00242 for key in purgelist:
00243 del dict[key]
00244
00245 def cleanup(self):
00246
00247 self.mutex.acquire()
00248 try:
00249 self.active_sounds = 0
00250 self.cleanupdict(self.filesounds)
00251 self.cleanupdict(self.voicesounds)
00252 self.cleanupdict(self.builtinsounds)
00253 except:
00254 rospy.loginfo('Exception in cleanup: %s'%sys.exc_info()[0])
00255 finally:
00256 self.mutex.release()
00257
00258 def diagnostics(self, state):
00259 try:
00260 da = DiagnosticArray()
00261 ds = DiagnosticStatus()
00262 ds.name = rospy.get_caller_id().lstrip('/') + ": Node State"
00263 if state == 0:
00264 ds.level = DiagnosticStatus.OK
00265 ds.message = "%i sounds playing"%self.active_sounds
00266 ds.values.append(KeyValue("Active sounds", str(self.active_sounds)))
00267 ds.values.append(KeyValue("Allocated sound channels", str(self.num_channels)))
00268 ds.values.append(KeyValue("Buffered builtin sounds", str(len(self.builtinsounds))))
00269 ds.values.append(KeyValue("Buffered wave sounds", str(len(self.filesounds))))
00270 ds.values.append(KeyValue("Buffered voice sounds", str(len(self.voicesounds))))
00271 elif state == 1:
00272 ds.level = DiagnosticStatus.WARN
00273 ds.message = "Sound device not open yet."
00274 else:
00275 ds.level = DiagnosticStatus.ERROR
00276 ds.message = "Can't open sound device. See http://pr.willowgarage.com/wiki/sound_play/Troubleshooting"
00277 da.status.append(ds)
00278 da.header.stamp = rospy.get_rostime()
00279 self.diagnostic_pub.publish(da)
00280 except Exception, e:
00281 rospy.loginfo('Exception in diagnostics: %s'%str(e))
00282
00283 def __init__(self):
00284 rospy.init_node('sound_play')
00285 self.diagnostic_pub = rospy.Publisher("/diagnostics", DiagnosticArray)
00286
00287 rootdir = os.path.join(os.path.dirname(__file__),'..','sounds')
00288
00289 self.builtinsoundparams = {
00290 SoundRequest.BACKINGUP : (os.path.join(rootdir, 'BACKINGUP.ogg'), 0.1),
00291 SoundRequest.NEEDS_UNPLUGGING : (os.path.join(rootdir, 'NEEDS_UNPLUGGING.ogg'), 1),
00292 SoundRequest.NEEDS_PLUGGING : (os.path.join(rootdir, 'NEEDS_PLUGGING.ogg'), 1),
00293 SoundRequest.NEEDS_UNPLUGGING_BADLY : (os.path.join(rootdir, 'NEEDS_UNPLUGGING_BADLY.ogg'), 1),
00294 SoundRequest.NEEDS_PLUGGING_BADLY : (os.path.join(rootdir, 'NEEDS_PLUGGING_BADLY.ogg'), 1),
00295 }
00296
00297 self.mutex = threading.Lock()
00298 sub = rospy.Subscriber("robotsound", SoundRequest, self.callback)
00299 self.mutex.acquire()
00300 self.no_error = True
00301 self.initialized = False
00302 self.active_sounds = 0
00303 self.sleep(0.5)
00304 self.diagnostics(1)
00305 while not rospy.is_shutdown():
00306 while not rospy.is_shutdown() and self.mixer_init():
00307 self.no_error = True
00308 self.initialized = True
00309 self.mutex.release()
00310 try:
00311 self.idle_loop()
00312
00313
00314 except:
00315 rospy.loginfo('Exception in idle_loop: %s'%sys.exc_info()[0])
00316 finally:
00317 self.mutex.acquire()
00318 mixer.quit()
00319 self.diagnostics(2)
00320 self.mutex.release()
00321
00322 def mixer_init(self):
00323 try:
00324 mixer.init(11025, -16, 1, 4000)
00325 self.init_vars()
00326 return True
00327 except Exception, e:
00328 if self.no_error:
00329 rospy.logerr('Exception in sound startup, will retry once per second. Is the speaker connected? Have you configured ALSA? Can aplay play sound? See the wiki if there is a red light on the Logitech speaker. Have a look at http://pr.willowgarage.com/wiki/sound_play/Troubleshooting Error message: %s'%str(e))
00330 self.no_error = False
00331 self.initialized = False
00332 self.sleep(1);
00333 return False
00334
00335 def init_vars(self):
00336 self.num_channels = 10
00337 mixer.set_num_channels(self.num_channels)
00338 self.builtinsounds = {}
00339 self.filesounds = {}
00340 self.voicesounds = {}
00341 self.hotlist = []
00342 if not self.initialized:
00343 rospy.loginfo('sound_play node is ready to play sound')
00344
00345 def sleep(self, duration):
00346 try:
00347 rospy.sleep(duration)
00348 except rospy.exceptions.ROSInterruptException:
00349 pass
00350
00351 def idle_loop(self):
00352 self.last_activity_time = rospy.get_time()
00353 while (rospy.get_time() - self.last_activity_time < 10 or
00354 len(self.builtinsounds) + len(self.voicesounds) + len(self.filesounds) > 0) \
00355 and not rospy.is_shutdown():
00356
00357 self.diagnostics(0)
00358 self.sleep(1)
00359 self.cleanup()
00360
00361
00362 if __name__ == '__main__':
00363 soundplay()
00364