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