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
00040 import rospy
00041 import threading
00042 import os
00043 import logging
00044 import sys
00045 import traceback
00046 import tempfile
00047 from diagnostic_msgs.msg import DiagnosticStatus, KeyValue, DiagnosticArray
00048 from sound_play.msg import SoundRequest, SoundRequestAction, SoundRequestResult, SoundRequestFeedback
00049 import actionlib
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 self.bus = self.sound.get_bus()
00097 self.bus.add_signal_watch()
00098 self.bus.connect("message", self.on_stream_end)
00099
00100 def on_stream_end(self, bus, message):
00101 if message.type == gst.MESSAGE_EOS:
00102 self.state = self.STOPPED
00103
00104 def __del__(self):
00105
00106 self.stop()
00107
00108 def update(self):
00109 self.bus.poll(gst.MESSAGE_ERROR, 10)
00110
00111 def loop(self):
00112 self.lock.acquire()
00113 try:
00114 self.staleness = 0
00115 if self.state == self.COUNTING:
00116 self.stop()
00117
00118 if self.state == self.STOPPED:
00119 self.sound.seek_simple(gst.FORMAT_TIME, gst.SEEK_FLAG_FLUSH, 0)
00120 self.sound.set_state(gst.STATE_PLAYING)
00121 self.state = self.LOOPING
00122 finally:
00123 self.lock.release()
00124
00125 def stop(self):
00126 if self.state != self.STOPPED:
00127 self.lock.acquire()
00128 try:
00129 self.sound.set_state(gst.STATE_NULL)
00130 self.state = self.STOPPED
00131 finally:
00132 self.lock.release()
00133
00134 def single(self):
00135 self.lock.acquire()
00136 try:
00137 rospy.logdebug("Playing %s"%self.uri)
00138 self.staleness = 0
00139 if self.state == self.LOOPING:
00140 self.stop()
00141
00142 self.sound.seek_simple(gst.FORMAT_TIME, gst.SEEK_FLAG_FLUSH, 0)
00143 self.sound.set_state(gst.STATE_PLAYING)
00144 self.state = self.COUNTING
00145 finally:
00146 self.lock.release()
00147
00148 def command(self, cmd):
00149 if cmd == SoundRequest.PLAY_STOP:
00150 self.stop()
00151 elif cmd == SoundRequest.PLAY_ONCE:
00152 self.single()
00153 elif cmd == SoundRequest.PLAY_START:
00154 self.loop()
00155
00156 def get_staleness(self):
00157 self.lock.acquire()
00158 position = 0
00159 duration = 0
00160 try:
00161 position = self.sound.query_position(gst.FORMAT_TIME)[0]
00162 duration = self.sound.query_duration(gst.FORMAT_TIME)[0]
00163 except Exception, e:
00164 position = 0
00165 duration = 0
00166 finally:
00167 self.lock.release()
00168
00169 if position != duration:
00170 self.staleness = 0
00171 else:
00172 self.staleness = self.staleness + 1
00173 return self.staleness
00174
00175 def get_playing(self):
00176 return self.state == self.COUNTING
00177
00178 class soundplay:
00179 _feedback = SoundRequestFeedback()
00180 _result = SoundRequestResult()
00181
00182 def stopdict(self,dict):
00183 for sound in dict.values():
00184 sound.stop()
00185
00186 def stopall(self):
00187 self.stopdict(self.builtinsounds)
00188 self.stopdict(self.filesounds)
00189 self.stopdict(self.voicesounds)
00190
00191 def select_sound(self, data):
00192 if data.sound == SoundRequest.PLAY_FILE:
00193 if not data.arg2:
00194 if not data.arg in self.filesounds.keys():
00195 rospy.logdebug('command for uncached wave: "%s"'%data.arg)
00196 try:
00197 self.filesounds[data.arg] = soundtype(data.arg)
00198 except:
00199 rospy.logerr('Error setting up to play "%s". Does this file exist on the machine on which sound_play is running?'%data.arg)
00200 return
00201 else:
00202 rospy.logdebug('command for cached wave: "%s"'%data.arg)
00203 sound = self.filesounds[data.arg]
00204 else:
00205 absfilename = os.path.join(roslib.packages.get_pkg_dir(data.arg2), data.arg)
00206 if not absfilename in self.filesounds.keys():
00207 rospy.logdebug('command for uncached wave: "%s"'%absfilename)
00208 try:
00209 self.filesounds[absfilename] = soundtype(absfilename)
00210 except:
00211 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))
00212 return
00213 else:
00214 rospy.logdebug('command for cached wave: "%s"'%absfilename)
00215 sound = self.filesounds[absfilename]
00216 elif data.sound == SoundRequest.SAY:
00217 if not data.arg in self.voicesounds.keys():
00218 rospy.logdebug('command for uncached text: "%s"' % data.arg)
00219 txtfile = tempfile.NamedTemporaryFile(prefix='sound_play', suffix='.txt')
00220 (wavfile,wavfilename) = tempfile.mkstemp(prefix='sound_play', suffix='.wav')
00221 txtfilename=txtfile.name
00222 os.close(wavfile)
00223 voice = data.arg2
00224 try:
00225 txtfile.write(data.arg)
00226 txtfile.flush()
00227 os.system("text2wave -eval '("+voice+")' "+txtfilename+" -o "+wavfilename)
00228 try:
00229 if os.stat(wavfilename).st_size == 0:
00230 raise OSError
00231 except OSError:
00232 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')
00233 return
00234 self.voicesounds[data.arg] = soundtype(wavfilename)
00235 finally:
00236 txtfile.close()
00237 else:
00238 rospy.logdebug('command for cached text: "%s"'%data.arg)
00239 sound = self.voicesounds[data.arg]
00240 else:
00241 rospy.logdebug('command for builtin wave: %i'%data.sound)
00242 if not data.sound in self.builtinsounds:
00243 params = self.builtinsoundparams[data.sound]
00244 self.builtinsounds[data.sound] = soundtype(params[0], params[1])
00245 sound = self.builtinsounds[data.sound]
00246 if sound.staleness != 0 and data.command != SoundRequest.PLAY_STOP:
00247
00248 rospy.logdebug("activating %i %s"%(data.sound,data.arg))
00249 self.active_sounds = self.active_sounds + 1
00250 sound.staleness = 0
00251
00252
00253
00254 return sound
00255
00256 def callback(self,data):
00257 if not self.initialized:
00258 return
00259 self.mutex.acquire()
00260
00261 self.stopall()
00262 try:
00263 if data.sound == SoundRequest.ALL and data.command == SoundRequest.PLAY_STOP:
00264 self.stopall()
00265 else:
00266 sound = self.select_sound(data)
00267 sound.command(data.command)
00268 except Exception, e:
00269 rospy.logerr('Exception in callback: %s'%str(e))
00270 rospy.loginfo(traceback.format_exc())
00271 finally:
00272 self.mutex.release()
00273 rospy.logdebug("done callback")
00274
00275
00276 def cleanupdict(self, dict):
00277 purgelist = []
00278 for (key,sound) in dict.iteritems():
00279 try:
00280 staleness = sound.get_staleness()
00281 except Exception, e:
00282 rospy.logerr('Exception in cleanupdict for sound (%s): %s'%(str(key),str(e)))
00283 staleness = 100
00284
00285 if staleness >= 10:
00286 purgelist.append(key)
00287 if staleness == 0:
00288 self.active_sounds = self.active_sounds + 1
00289 for key in purgelist:
00290 rospy.logdebug('Purging %s from cache'%key)
00291 del dict[key]
00292
00293 def cleanup(self):
00294 self.mutex.acquire()
00295 try:
00296 self.active_sounds = 0
00297 self.cleanupdict(self.filesounds)
00298 self.cleanupdict(self.voicesounds)
00299 self.cleanupdict(self.builtinsounds)
00300 except:
00301 rospy.loginfo('Exception in cleanup: %s'%sys.exc_info()[0])
00302 finally:
00303 self.mutex.release()
00304
00305 def diagnostics(self, state):
00306 try:
00307 da = DiagnosticArray()
00308 ds = DiagnosticStatus()
00309 ds.name = rospy.get_caller_id().lstrip('/') + ": Node State"
00310 if state == 0:
00311 ds.level = DiagnosticStatus.OK
00312 ds.message = "%i sounds playing"%self.active_sounds
00313 ds.values.append(KeyValue("Active sounds", str(self.active_sounds)))
00314 ds.values.append(KeyValue("Allocated sound channels", str(self.num_channels)))
00315 ds.values.append(KeyValue("Buffered builtin sounds", str(len(self.builtinsounds))))
00316 ds.values.append(KeyValue("Buffered wave sounds", str(len(self.filesounds))))
00317 ds.values.append(KeyValue("Buffered voice sounds", str(len(self.voicesounds))))
00318 elif state == 1:
00319 ds.level = DiagnosticStatus.WARN
00320 ds.message = "Sound device not open yet."
00321 else:
00322 ds.level = DiagnosticStatus.ERROR
00323 ds.message = "Can't open sound device. See http://pr.willowgarage.com/wiki/sound_play/Troubleshooting"
00324 da.status.append(ds)
00325 da.header.stamp = rospy.get_rostime()
00326 self.diagnostic_pub.publish(da)
00327 except Exception, e:
00328 rospy.loginfo('Exception in diagnostics: %s'%str(e))
00329
00330 def execute_cb(self, data):
00331 data = data.sound_request
00332 if not self.initialized:
00333 return
00334 self.mutex.acquire()
00335
00336 self.stopall()
00337 try:
00338 if data.sound == SoundRequest.ALL and data.command == SoundRequest.PLAY_STOP:
00339 self.stopall()
00340 else:
00341 sound = self.select_sound(data)
00342 sound.command(data.command)
00343
00344 r = rospy.Rate(1)
00345 start_time = rospy.get_rostime()
00346 success = True
00347 while sound.get_playing():
00348 sound.update()
00349 if self._as.is_preempt_requested():
00350 rospy.loginfo('sound_play action: Preempted')
00351 sound.stop()
00352 self._as.set_preempted()
00353 success = False
00354 break
00355
00356 self._feedback.playing = sound.get_playing()
00357 self._feedback.stamp = rospy.get_rostime() - start_time
00358 self._as.publish_feedback(self._feedback)
00359 r.sleep()
00360
00361 if success:
00362 self._result.playing = self._feedback.playing
00363 self._result.stamp = self._feedback.stamp
00364 rospy.loginfo('sound_play action: Succeeded')
00365 self._as.set_succeeded(self._result)
00366
00367 except Exception, e:
00368 rospy.logerr('Exception in actionlib callback: %s'%str(e))
00369 rospy.loginfo(traceback.format_exc())
00370 finally:
00371 self.mutex.release()
00372 rospy.logdebug("done actionlib callback")
00373
00374 def __init__(self):
00375 rospy.init_node('sound_play')
00376 self.diagnostic_pub = rospy.Publisher("/diagnostics", DiagnosticArray)
00377 rootdir = os.path.join(roslib.packages.get_pkg_dir('sound_play'),'sounds')
00378
00379 self.builtinsoundparams = {
00380 SoundRequest.BACKINGUP : (os.path.join(rootdir, 'BACKINGUP.ogg'), 0.1),
00381 SoundRequest.NEEDS_UNPLUGGING : (os.path.join(rootdir, 'NEEDS_UNPLUGGING.ogg'), 1),
00382 SoundRequest.NEEDS_PLUGGING : (os.path.join(rootdir, 'NEEDS_PLUGGING.ogg'), 1),
00383 SoundRequest.NEEDS_UNPLUGGING_BADLY : (os.path.join(rootdir, 'NEEDS_UNPLUGGING_BADLY.ogg'), 1),
00384 SoundRequest.NEEDS_PLUGGING_BADLY : (os.path.join(rootdir, 'NEEDS_PLUGGING_BADLY.ogg'), 1),
00385 }
00386
00387 self.mutex = threading.Lock()
00388 sub = rospy.Subscriber("robotsound", SoundRequest, self.callback)
00389 self._as = actionlib.SimpleActionServer('sound_play', SoundRequestAction, execute_cb=self.execute_cb, auto_start = False)
00390 self._as.start()
00391
00392 self.mutex.acquire()
00393 self.no_error = True
00394 self.initialized = False
00395 self.active_sounds = 0
00396 self.sleep(0.5)
00397 self.diagnostics(1)
00398
00399 while not rospy.is_shutdown():
00400 while not rospy.is_shutdown():
00401 self.init_vars()
00402 self.no_error = True
00403 self.initialized = True
00404 self.mutex.release()
00405 try:
00406 self.idle_loop()
00407
00408
00409 except:
00410 rospy.loginfo('Exception in idle_loop: %s'%sys.exc_info()[0])
00411 finally:
00412 self.mutex.acquire()
00413
00414 self.diagnostics(2)
00415 self.mutex.release()
00416
00417 def init_vars(self):
00418 self.num_channels = 10
00419 self.builtinsounds = {}
00420 self.filesounds = {}
00421 self.voicesounds = {}
00422 self.hotlist = []
00423 if not self.initialized:
00424 rospy.loginfo('sound_play node is ready to play sound')
00425
00426 def sleep(self, duration):
00427 try:
00428 rospy.sleep(duration)
00429 except rospy.exceptions.ROSInterruptException:
00430 pass
00431
00432 def idle_loop(self):
00433 self.last_activity_time = rospy.get_time()
00434 while (rospy.get_time() - self.last_activity_time < 10 or
00435 len(self.builtinsounds) + len(self.voicesounds) + len(self.filesounds) > 0) \
00436 and not rospy.is_shutdown():
00437
00438 self.diagnostics(0)
00439 self.sleep(1)
00440 self.cleanup()
00441
00442
00443 if __name__ == '__main__':
00444 soundplay()
00445