soundplay_node.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 #***********************************************************
00004 #* Software License Agreement (BSD License)
00005 #*
00006 #*  Copyright (c) 2009, Willow Garage, Inc.
00007 #*  All rights reserved.
00008 #*
00009 #*  Redistribution and use in source and binary forms, with or without
00010 #*  modification, are permitted provided that the following conditions
00011 #*  are met:
00012 #*
00013 #*   * Redistributions of source code must retain the above copyright
00014 #*     notice, this list of conditions and the following disclaimer.
00015 #*   * Redistributions in binary form must reproduce the above
00016 #*     copyright notice, this list of conditions and the following
00017 #*     disclaimer in the documentation and/or other materials provided
00018 #*     with the distribution.
00019 #*   * Neither the name of the Willow Garage nor the names of its
00020 #*     contributors may be used to endorse or promote products derived
00021 #*     from this software without specific prior written permission.
00022 #*
00023 #*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 #*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 #*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 #*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 #*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 #*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 #*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 #*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 #*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 #*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 #*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 #*  POSSIBILITY OF SUCH DAMAGE.
00035 #***********************************************************
00036 
00037 # Author: Blaise Gassend
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         # stop our GST object so that it gets garbage-collected
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 # So we hit the same catch block
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             # This sound isn't counted in active_sounds
00248             rospy.logdebug("activating %i %s"%(data.sound,data.arg))
00249             self.active_sounds = self.active_sounds + 1
00250             sound.staleness = 0
00251             #                    if self.active_sounds > self.num_channels:
00252             #                        mixer.set_num_channels(self.active_sounds)
00253             #                        self.num_channels = self.active_sounds
00254         return sound
00255 
00256     def callback(self,data):
00257         if not self.initialized:
00258             return
00259         self.mutex.acquire()
00260         # Force only one sound at a time
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     # Purge sounds that haven't been played in a while.
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 # Something is wrong. Let's purge and try again.
00284             #print "%s %i"%(key, staleness)
00285             if staleness >= 10:
00286                 purgelist.append(key)
00287             if staleness == 0: # Sound is playing
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         # Force only one sound at a time
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) # For ros startup race condition
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                     # Returns after inactive period to test device availability
00408                     #print "Exiting idle"
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             #print "idle_loop"
00438             self.diagnostics(0)
00439             self.sleep(1)
00440             self.cleanup()
00441         #print "idle_exiting"
00442 
00443 if __name__ == '__main__':
00444     soundplay()
00445 


sound_play
Author(s): Blaise Gassend
autogenerated on Wed Sep 2 2015 01:21:46