00001
00002
00003 import roslib
00004 roslib.load_manifest('cob_sound')
00005 import rospy
00006
00007 import os
00008
00009 import actionlib
00010
00011 from std_msgs.msg import String
00012 from cob_sound.msg import *
00013 from cob_sound.srv import *
00014
00015 class SoundAction(object):
00016
00017 def __init__(self):
00018 self._as = actionlib.SimpleActionServer("say", SayAction, execute_cb=self.as_cb)
00019 rospy.Subscriber("/say", String, self.topic_cb)
00020 rospy.Service('/say', SayText, self.service_cb)
00021 self._as.start()
00022
00023 def as_cb(self, goal):
00024 print "begin"
00025 self.say(goal.text.data)
00026 self._as.set_succeeded()
00027 print "end"
00028
00029 def topic_cb(self,msg):
00030 self.say(msg.data)
00031
00032 def service_cb(self,msg):
00033 self.say(msg.text)
00034 res = SayTextResponse()
00035 return res
00036
00037 def say(self,text):
00038 rospy.loginfo('Saying: %s' % (text))
00039 os.system("echo " + text + " | text2wave | aplay -q &")
00040
00041 if __name__ == '__main__':
00042 rospy.init_node('cob_sound')
00043 SoundAction()
00044 rospy.loginfo("cob_sound is running")
00045 rospy.spin()
00046