def __init__(self, phrase_file): self.sound_client = SoundClient() rospy.sleep(0.5) self.sound_client.stopAll() #self.animation_client = actionlib.SimpleActionClient('/Animation_Server',ExpressionMotionAction) self.viseme_client = actionlib.SimpleActionClient( '/Viseme_Server', VisemeAction) #self.lookat_client = actionlib.SimpleActionClient('/Lookat_Server',LookatAction) rospy.loginfo("Waiting for Cordial Action Servers") #rospy.loginfo(" --- Animation") #self.animation_client.wait_for_server() rospy.loginfo(" --- Visemes") self.viseme_client.wait_for_server() #rospy.loginfo(" --- Lookat") #self.lookat_client.wait_for_server() rospy.loginfo("Action servers connected") rospy.loginfo("Reading Phrase File") with open(phrase_file, 'r') as f: s = f.read() self.phrases = yaml.load(s) self.feedback.viseme = "IDLE" self.feedback.action = "none" rospy.loginfo("Starting server...") self.server = actionlib.SimpleActionServer('SBPlayback_Server', SpeechPlayAction, execute_cb=self.execute_cb) self.server.start() rospy.loginfo("Server started")
def __init__(self, dm, tm, info, dialogue_info): smach.State.__init__(self, outcomes=['continue', 'end', 'timeout', 'panic']) self.sc = SoundClient() self.dm = dm self.tm = tm self.exp_start = rospy.get_param("~start_time") self.duration = rospy.get_param("~max_time") self.break_time = rospy.Duration(60) self.encouragement_every = 10 self.dialogue = dialogue_info #self.workout_phrases = workout_info self.day, self.lesson = info self.gui_prefix = "dragon_GUI/" self.seen_victory = False self.music_folder = roslib.packages.get_pkg_dir("expeditions_year1")+ "/music/" #pose is: x, y, z, (theta, neck, vel, acc [optional]) self.poses = {'right': (0, 2.4, .5), 'left': (0, -2.4, .5), 'up': (0, 0, 3), 'down': (0, 0, 0)} # song and bpm self.songs = {'mario_yoshi.wav':104, 'Donkey_Kong_Country_Jungle_Stomp_OC_ReMix.wav':84, 'Legend_of_Zelda_A_Link_to_the_Past_Kakariko_Rave_Party_OC_ReMix.wav':160, 'Legend_of_Zelda_Ocarina_of_Time_This_Valley_Rocks_OC_ReMix.wav':96, 'Super_Mario_World_2_Yoshi\'s_Island_Dino_Band_Rehearsal_OC_ReMix.wav':112, 'Super_Mario_World_Swanky_Vegas_OC_ReMix.wav':120} self.current_song = "" self.dg = DialogueManager(self.dm, self.tm, self.day + "_workout", self.dialogue) self.vol = 0.3
def __init__(self, phrase_file): self.sound_client = SoundClient() rospy.sleep(0.5) self.sound_client.stopAll() # self.animation_client = actionlib.SimpleActionClient('/Animation_Server',ExpressionMotionAction) self.viseme_client = actionlib.SimpleActionClient("/Viseme_Server", VisemeAction) # self.lookat_client = actionlib.SimpleActionClient('/Lookat_Server',LookatAction) rospy.loginfo("Waiting for Cordial Action Servers") # rospy.loginfo(" --- Animation") # self.animation_client.wait_for_server() rospy.loginfo(" --- Visemes") self.viseme_client.wait_for_server() # rospy.loginfo(" --- Lookat") # self.lookat_client.wait_for_server() rospy.loginfo("Action servers connected") rospy.loginfo("Reading Phrase File") with open(phrase_file, "r") as f: s = f.read() self.phrases = yaml.load(s) self.feedback.viseme = "IDLE" self.feedback.action = "none" rospy.loginfo("Starting server...") self.server = actionlib.SimpleActionServer("SBPlayback_Server", SpeechPlayAction, execute_cb=self.execute_cb) self.server.start() rospy.loginfo("Server started")
def __init__(self, dm, tm, exp_info, dialogue_info, food_phrases): smach.State.__init__(self, outcomes=['panic', 'next_round', 'end', 'timeout']) self.dm = dm self.tm = tm self.ntimes = 0 self.day, self.lessons = exp_info self.fp = food_phrases[self.day] self.all_foods = self.fp["bad"] + self.fp["good"] + self.fp["sometimes"] self.dialogue = dialogue_info self.gui_prefix = "dragon_GUI/" self.dg = DialogueManager(self.dm, self.tm, self.day + "_foods", self.dialogue) self.sc = SoundClient() self.music_folder = roslib.packages.get_pkg_dir("expeditions_year1")+ "/music/" self.feedback_levels = {i:{"good":0,"bad":0} for i in self.fp["groups"]} self.feedback_levels["all"] = {"good":0, "bad":0} self.feedback_levels["reminders"] = 0 self.prev_items = dict(good = [], bad = [], sometimes = []) self.target_group = "all" self.selected_foods = [] self.sometimes_feedback = {food:False for food in self.fp["sometimes"]} self.exp_start_time = rospy.get_param("~start_time") self.duration = rospy.get_param("~max_time") self.seen_bad = set()
def __init__(self): def callback(data): if int(data.data) == 100: self.performedBehaviors = [] return #action behavior processing if int(data.data) == 0 and len(self.performedBehaviors) > 1: self.performedBehaviors = [] if data not in self.performedBehaviors: print("performing behavior") # time.sleep(1.0) case = int(data.data) if case == 0: self.robot.say("demo2", wait=True) elif case == 1: self.robot.say("dance1", wait=True) # time.sleep(4.0) elif case == 2: self.robot.say("compliment1", wait=True) elif case == 3: self.robot.say("encouragement1", wait=True) elif case == 4: self.robot.say("sassy1", wait=True) elif case == 5: self.robot.say("joke1", wait=True) elif case == 6: self.sound.playWave( '/home/saunter/rosbuild_ws/saunter/saunter_interaction/speech/data/DanceSong.wav' ) self.robot.do("shimmy", wait=True) # time.sleep(4.0) self.performedBehaviors += [data] self.robot.do("returnToNeutral", wait=True) rospy.Publisher("kiwi", String, queue_size=2).publish("behavior completed") # In ROS, nodes are uniquely named. If two nodes with the same # node are launched, the previous one is kicked off. The # anonymous=True flag means that rospy will choose a unique # name for our 'listener' node so that multiple listeners can # run simultaneously. rospy.init_node('BehaviorServer', anonymous=True) rospy.Subscriber("behavior_number", String, callback, queue_size=4) self.robot = RobotManager("DB1") self.performedBehaviors = [] self.sound = SoundClient()
def __init__(self): self.sound_client = SoundClient() rospy.sleep(0.5) self.sound_client.stopAll() self.phrases = {} self.expressions = ["angry", "frightened", "puppy", "sipping", "confused", "lovestruck", "sad", "tasting", "disgusted", "mischievous", "surprised"] self.motions = ["afraid", "blech", "idunno", "interest", "mmhmmm", "question", "wakeup", "yes", "anticipation", "farted", "i_like_it", "ilikeit", "laugh1", "mph", "surprise", "yay", "yummm", "bite", "hungry", "i_want_it", "iwantit", "meh", "no", "think", "yawn"]
def __init__(self, phrase_file): self.sound_client = SoundClient() rospy.sleep(0.5) self.sound_client.stopAll() self.express_client = actionlib.SimpleActionClient('/ExpressionMotion_Server',ExpressionMotionAction) self.viseme_client = actionlib.SimpleActionClient('/Viseme_Server',VisemeAction) self.ik_client = actionlib.SimpleActionClient('/IK_Server',IKAction) self.lookat_client = actionlib.SimpleActionClient('/Lookat_Server',LookatAction) self.track_client = actionlib.SimpleActionClient('/Track_Server',TrackAction) self.blink_client = actionlib.SimpleActionClient('/Blink_Server',BlinkAction) rospy.loginfo("Waiting for Dragonbot Action Servers") rospy.loginfo(" --- Expression Motion") self.express_client.wait_for_server() rospy.loginfo(" --- Visemes") self.viseme_client.wait_for_server() rospy.loginfo(" --- IK") self.ik_client.wait_for_server() rospy.loginfo(" --- Lookat") self.lookat_client.wait_for_server() rospy.loginfo(" --- TF Tracking") self.track_client.wait_for_server() rospy.loginfo(" --- Blinking") self.blink_client.wait_for_server() rospy.loginfo("Action servers connected") with open(phrase_file, 'r') as f: s = f.read() self.phrases = yaml.load(s) self.feedback.viseme = "IDLE" self.feedback.action = "none" rospy.loginfo("Starting server...") self.server = actionlib.SimpleActionServer('SBPlayback_Server', SpeechPlayAction, execute_cb=self.execute_cb) self.server.start()
#* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE #* POSSIBILITY OF SUCH DAMAGE. #*********************************************************** # Author: Blaise Gassend import roslib; roslib.load_manifest('cordial_sound') import rospy from cordial_sound.msg import SoundRequest from cordial_sound.libsoundplay import SoundClient if __name__ == '__main__': rospy.init_node('shutup', anonymous = True) soundhandle = SoundClient() rospy.sleep(0.5) # let ROS get started... print "Sending stopAll commande every 100 ms." print "Note: This will not prevent a node that is continuing to issue commands" print "from producing sound." print "Press Ctrl+C to exit." while not rospy.is_shutdown(): soundhandle.stopAll() try: rospy.sleep(.1) except: pass
import sys if __name__ == "__main__": if len(sys.argv) != 2 or sys.argv[1] == "--help": print "Usage: %s <sound_id>" % sys.argv[0] print print "Plays one of the built-in sounds based on its integer ID. Look at the cordial_sound/SoundRequest message definition for IDs." exit(1) # Import here so that usage is fast. import roslib roslib.load_manifest("cordial_sound") import rospy from cordial_sound.msg import SoundRequest from cordial_sound.libsoundplay import SoundClient rospy.init_node("play", anonymous=True) soundhandle = SoundClient() rospy.sleep(1) num = int(sys.argv[1]) print "Playing sound %i." % num soundhandle.play(num) rospy.sleep(1)
print 'appropriate. For a string on standard input, the command will wait for' print 'EOF before saying anything.' exit(-1) # Import after printing usage for speed. import roslib; roslib.load_manifest('cordial_sound') import rospy from cordial_sound.msg import SoundRequest from cordial_sound.libsoundplay import SoundClient if len(sys.argv) == 1: print 'Awaiting something to say on standard input.' # Ordered this way to minimize wait time. rospy.init_node('say', anonymous = True) soundhandle = SoundClient() rospy.sleep(1) voice = 'voice_kal_diphone' if len(sys.argv) == 1: s = sys.stdin.read() else: s = sys.argv[1] if len(sys.argv) == 3: voice = sys.argv[2] print 'Saying: %s' % s print 'Voice: %s' % voice soundhandle.say(s,voice)
class SpeechPlayServer(): feedback = SpeechPlayFeedback() result = SpeechPlayResult() def __init__(self, phrase_file): self.sound_client = SoundClient() rospy.sleep(0.5) self.sound_client.stopAll() #self.animation_client = actionlib.SimpleActionClient('/Animation_Server',ExpressionMotionAction) self.viseme_client = actionlib.SimpleActionClient( '/Viseme_Server', VisemeAction) #self.lookat_client = actionlib.SimpleActionClient('/Lookat_Server',LookatAction) rospy.loginfo("Waiting for Cordial Action Servers") #rospy.loginfo(" --- Animation") #self.animation_client.wait_for_server() rospy.loginfo(" --- Visemes") self.viseme_client.wait_for_server() #rospy.loginfo(" --- Lookat") #self.lookat_client.wait_for_server() rospy.loginfo("Action servers connected") rospy.loginfo("Reading Phrase File") with open(phrase_file, 'r') as f: s = f.read() self.phrases = yaml.load(s) self.feedback.viseme = "IDLE" self.feedback.action = "none" rospy.loginfo("Starting server...") self.server = actionlib.SimpleActionServer('SBPlayback_Server', SpeechPlayAction, execute_cb=self.execute_cb) self.server.start() rospy.loginfo("Server started") def execute_cb(self, goal): rospy.loginfo("Phrase playing: " + str(goal.phrase)) preempted = False if goal.interrupt == True: rospy.loginfo("Speech: cancelling all goals") self.animation_client.cancel_all_goals() self.viseme_client.cancel_all_goals() self.sound_client.stopAll() self.lookat_client.cancel_all_goals() timing_adjust = rospy.Duration.from_sec(0.2) try: actions = self.phrases[goal.phrase]["actions"] except KeyError: rospy.logerr("Phrase id %s not recognized" % goal.phrase) time = rospy.Time.now() ordered_actions = sorted(actions, key=lambda action: action["start"]) #self.sound_client.stopAll() wave_file = self.phrases[goal.phrase]["file"] wave_duration = 0.0 with contextlib.closing(wave.open(wave_file, 'r')) as f: frames = f.getnframes() rate = f.getframerate() wave_duration = frames / float(rate) rospy.loginfo("Speech: playing wave file -- duration: " + str(wave_duration)) self.sound_client.playWave(wave_file) for a in ordered_actions: rospy.loginfo("Playing action: " + str(a)) if a["type"] == "viseme": while rospy.Time.now( ) - time + timing_adjust < rospy.Duration.from_sec( a["start"]) and not self.server.is_preempt_requested(): pass if self.server.is_preempt_requested(): preempted = True break if a["type"] == "viseme" and ( rospy.Time.now() - time + timing_adjust > rospy.Duration.from_sec(a["end"])): continue if a["type"] == "viseme": #rospy.loginfo("Viseme: " + a["id"]) vgoal = dragon_msgs.msg.VisemeGoal(constant=a["id"]) self.viseme_client.send_goal(vgoal) self.feedback.viseme = a["id"] self.server.publish_feedback(self.feedback) else: rospy.logwarn("action not recognized") '''elif a["type"] == "animation": rospy.loginfo("Expression: " + a["id"]) egoal = cordial_msgs.msg.AnimationGoal(name=a["id"],constant=a["id"]) self.express_client.send_goal(egoal) self.feedback.action = a["id"] self.server.publish_feedback(self.feedback) elif a["type"] == "lookat": rospy.loginfo("Lookat: target: " + a["target"]) lgoal = dragon_msgs.msg.LookatGoal(target=a["target"], target_duration=rospy.Duration.from_sec(float(a["duration"]))) self.lookat_client.send_goal(lgoal) elif a["type"] == "track": rospy.loginfo("Track: " + a["target"]) tgoal = dragon_msgs.msg.TrackGoal(on = True, target = a["target"]) self.track_client.send_goal(tgoal) else: rospy.logwarn("action not recognized")''' if preempted: rospy.loginfo("Preempted") #self.express_client.cancel_all_goals() self.viseme_client.cancel_all_goals() self.sound_client.stopAll() self.server.set_preempted() return else: rospy.loginfo("Waiting for end") while (rospy.Time.now() - time + timing_adjust) < rospy.Duration.from_sec(wave_duration): pass rospy.loginfo("At end -- Success") self.result.result = "SUCCESS" self.viseme_client.cancel_all_goals() self.server.set_succeeded(self.result) return rospy.logwarn( "Speech: the code should probably never reach this point. Setting succeeded anyway." ) self.result.result = "SUCCESS" self.server.set_succeeded(self.result)
class SpeechPlayServer: feedback = SpeechPlayFeedback() result = SpeechPlayResult() def __init__(self, phrase_file): self.sound_client = SoundClient() rospy.sleep(0.5) self.sound_client.stopAll() # self.animation_client = actionlib.SimpleActionClient('/Animation_Server',ExpressionMotionAction) self.viseme_client = actionlib.SimpleActionClient("/Viseme_Server", VisemeAction) # self.lookat_client = actionlib.SimpleActionClient('/Lookat_Server',LookatAction) rospy.loginfo("Waiting for Cordial Action Servers") # rospy.loginfo(" --- Animation") # self.animation_client.wait_for_server() rospy.loginfo(" --- Visemes") self.viseme_client.wait_for_server() # rospy.loginfo(" --- Lookat") # self.lookat_client.wait_for_server() rospy.loginfo("Action servers connected") rospy.loginfo("Reading Phrase File") with open(phrase_file, "r") as f: s = f.read() self.phrases = yaml.load(s) self.feedback.viseme = "IDLE" self.feedback.action = "none" rospy.loginfo("Starting server...") self.server = actionlib.SimpleActionServer("SBPlayback_Server", SpeechPlayAction, execute_cb=self.execute_cb) self.server.start() rospy.loginfo("Server started") def execute_cb(self, goal): rospy.loginfo("Phrase playing: " + str(goal.phrase)) preempted = False if goal.interrupt == True: rospy.loginfo("Speech: cancelling all goals") self.animation_client.cancel_all_goals() self.viseme_client.cancel_all_goals() self.sound_client.stopAll() self.lookat_client.cancel_all_goals() timing_adjust = rospy.Duration.from_sec(0.2) try: actions = self.phrases[goal.phrase]["actions"] except KeyError: rospy.logerr("Phrase id %s not recognized" % goal.phrase) time = rospy.Time.now() ordered_actions = sorted(actions, key=lambda action: action["start"]) # self.sound_client.stopAll() wave_file = self.phrases[goal.phrase]["file"] wave_duration = 0.0 with contextlib.closing(wave.open(wave_file, "r")) as f: frames = f.getnframes() rate = f.getframerate() wave_duration = frames / float(rate) rospy.loginfo("Speech: playing wave file -- duration: " + str(wave_duration)) self.sound_client.playWave(wave_file) for a in ordered_actions: rospy.loginfo("Playing action: " + str(a)) if a["type"] == "viseme": while ( rospy.Time.now() - time + timing_adjust < rospy.Duration.from_sec(a["start"]) and not self.server.is_preempt_requested() ): pass if self.server.is_preempt_requested(): preempted = True break if a["type"] == "viseme" and (rospy.Time.now() - time + timing_adjust > rospy.Duration.from_sec(a["end"])): continue if a["type"] == "viseme": # rospy.loginfo("Viseme: " + a["id"]) vgoal = dragon_msgs.msg.VisemeGoal(constant=a["id"]) self.viseme_client.send_goal(vgoal) self.feedback.viseme = a["id"] self.server.publish_feedback(self.feedback) else: rospy.logwarn("action not recognized") """elif a["type"] == "animation": rospy.loginfo("Expression: " + a["id"]) egoal = cordial_msgs.msg.AnimationGoal(name=a["id"],constant=a["id"]) self.express_client.send_goal(egoal) self.feedback.action = a["id"] self.server.publish_feedback(self.feedback) elif a["type"] == "lookat": rospy.loginfo("Lookat: target: " + a["target"]) lgoal = dragon_msgs.msg.LookatGoal(target=a["target"], target_duration=rospy.Duration.from_sec(float(a["duration"]))) self.lookat_client.send_goal(lgoal) elif a["type"] == "track": rospy.loginfo("Track: " + a["target"]) tgoal = dragon_msgs.msg.TrackGoal(on = True, target = a["target"]) self.track_client.send_goal(tgoal) else: rospy.logwarn("action not recognized")""" if preempted: rospy.loginfo("Preempted") # self.express_client.cancel_all_goals() self.viseme_client.cancel_all_goals() self.sound_client.stopAll() self.server.set_preempted() return else: rospy.loginfo("Waiting for end") while (rospy.Time.now() - time + timing_adjust) < rospy.Duration.from_sec(wave_duration): pass rospy.loginfo("At end -- Success") self.result.result = "SUCCESS" self.viseme_client.cancel_all_goals() self.server.set_succeeded(self.result) return rospy.logwarn("Speech: the code should probably never reach this point. Setting succeeded anyway.") self.result.result = "SUCCESS" self.server.set_succeeded(self.result)
#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE #* POSSIBILITY OF SUCH DAMAGE. #*********************************************************** # Author: Blaise Gassend import sys if __name__ == '__main__': if len(sys.argv) != 2 or sys.argv[1] == '--help': print 'Usage: %s sound_to_play.(ogg|wav)'%sys.argv[0] print print 'Plays an .OGG or .WAV file. The path to the file should be absolute, and be valid on the computer on which cordial_sound is running.' exit(1) # Import after printing usage for speed. import roslib; roslib.load_manifest('cordial_sound') import rospy from cordial_sound.msg import SoundRequest from cordial_sound.libsoundplay import SoundClient rospy.init_node('play', anonymous = True) soundhandle = SoundClient() rospy.sleep(1) print 'Playing "%s".'%sys.argv[1] soundhandle.playWave(sys.argv[1]) rospy.sleep(1)
class DragonbotManager(): def __init__(self): self.sound_client = SoundClient() rospy.sleep(0.5) self.sound_client.stopAll() self.phrases = {} self.expressions = ["angry", "frightened", "puppy", "sipping", "confused", "lovestruck", "sad", "tasting", "disgusted", "mischievous", "surprised"] self.motions = ["afraid", "blech", "idunno", "interest", "mmhmmm", "question", "wakeup", "yes", "anticipation", "farted", "i_like_it", "ilikeit", "laugh1", "mph", "surprise", "yay", "yummm", "bite", "hungry", "i_want_it", "iwantit", "meh", "no", "think", "yawn"] def track_frame(self, frame_name): print "Tracking frame: " + frame_name def track_off(self): print "Tracking off." def say(self, phrase_name, interrupt=True, wait = False): if interrupt: self.sound_client.stopAll() if phrase_name in self.phrases: print "Saying phrase: " + phrase_name print "=====================================" self.sound_client.playWave(self.phrases[phrase_name]["file"]) print self.phrases[phrase_name]["text"] print "-------------------------------------" if wait: print "PRESS ENTER KEY WHEN SPEECH DONE" raw_input() else: print "Saying (unloaded) phrase: " + phrase_name #loads phrases from a yaml file # format: # phrases (list) # text (str) #for reference # file (str) #file with audio # visemes (list) # type (string) # start (float) # end (float) # actions (list) # type (string) # time (float) def load_phrases(self, filename): f = open(filename, 'r') s = f.read() self.phrases = yaml.load(s) def express(self, expression_id, expression_type = None, wait = True): recognized = False if expression_type == "expression" or expression_type == None: if expression_id in self.expressions: goal = dragon_msgs.msg.ExpressionMotionGoal(type='expression',constant=expression_id) recognized = True else: rospy.logwarn("Expression not recognized") if expression_type == "motion" or expression_type == None: if expression_id in self.motions: goal = dragon_msgs.msg.ExpressionMotionGoal(type='motion',constant=expression_id) recognizsed = True else: rospy.logwarn("Motion not recognized") if recognized: print "Playing expression/motion: " + expression_id def pose(self, vel, acc, x, y, z, theta, neck): print "Moving to pose with vel: {vel} acc: {acc} position: ({x},{y},{z}) theta: {theta} neck: {neck}".format(vel = vel, acc = acc, x = x, y = y, z = z, theta = theta, neck = neck) def lookat(self, x, y, z): # range for x: -300,300 # range for y: -300,300 # range for z: 20,400 print "Looking at: ({x},{y},{z})".format(x = x, y = y, z = z) def lookat_off(self): print "Lookat off" def lookat_frame(self, frame): print "Looking at frame: " + frame def eye_close(self): print "Eyes closed" def eye_open(self): print "Eyes open" def blink(self): print "Blink" def stop_speech(self): self.sound_client.stopAll()
class SpeechPlayServer(): feedback = SpeechPlayFeedback() result = SpeechPlayResult() def __init__(self, phrase_file): self.sound_client = SoundClient() rospy.sleep(0.5) self.sound_client.stopAll() self.express_client = actionlib.SimpleActionClient('/ExpressionMotion_Server',ExpressionMotionAction) self.viseme_client = actionlib.SimpleActionClient('/Viseme_Server',VisemeAction) self.ik_client = actionlib.SimpleActionClient('/IK_Server',IKAction) self.lookat_client = actionlib.SimpleActionClient('/Lookat_Server',LookatAction) self.track_client = actionlib.SimpleActionClient('/Track_Server',TrackAction) self.blink_client = actionlib.SimpleActionClient('/Blink_Server',BlinkAction) rospy.loginfo("Waiting for Dragonbot Action Servers") rospy.loginfo(" --- Expression Motion") self.express_client.wait_for_server() rospy.loginfo(" --- Visemes") self.viseme_client.wait_for_server() rospy.loginfo(" --- IK") self.ik_client.wait_for_server() rospy.loginfo(" --- Lookat") self.lookat_client.wait_for_server() rospy.loginfo(" --- TF Tracking") self.track_client.wait_for_server() rospy.loginfo(" --- Blinking") self.blink_client.wait_for_server() rospy.loginfo("Action servers connected") with open(phrase_file, 'r') as f: s = f.read() self.phrases = yaml.load(s) self.feedback.viseme = "IDLE" self.feedback.action = "none" rospy.loginfo("Starting server...") self.server = actionlib.SimpleActionServer('SBPlayback_Server', SpeechPlayAction, execute_cb=self.execute_cb) self.server.start() def execute_cb(self, goal): rospy.loginfo("Phrase playing: " + str(goal.phrase)) preempted = False if goal.interrupt == True: rospy.loginfo("Speech: cancelling all goals") #self.express_client.cancel_all_goals() self.viseme_client.cancel_all_goals() self.sound_client.stopAll() lgoal = dragon_msgs.msg.LookatGoal(state = "off") self.lookat_client.send_goal(lgoal) tgoal = dragon_msgs.msg.TrackGoal(on = False) self.track_client.send_goal(tgoal) timing_adjust = rospy.Duration.from_sec(0.2) try: actions = self.phrases[goal.phrase]["actions"] except KeyError: rospy.logerr("Key %s not recognized"%goal.phrase) time = rospy.Time.now() ordered_actions = sorted(actions, key=lambda action: action["start"]) #self.sound_client.stopAll() wave_file = self.phrases[goal.phrase]["file"] wave_duration = 0.0 with contextlib.closing(wave.open(wave_file,'r')) as f: frames=f.getnframes() rate=f.getframerate() wave_duration=frames/float(rate) rospy.loginfo("Speech: playing wave file -- duration: " + str(wave_duration)) self.sound_client.playWave(wave_file) for a in ordered_actions: rospy.loginfo("Playing action: " + str(a)) if a["type"] == "viseme": while rospy.Time.now()-time+timing_adjust < rospy.Duration.from_sec(a["start"]) and not self.server.is_preempt_requested(): pass if self.server.is_preempt_requested(): preempted = True break if a["type"] == "viseme" and (rospy.Time.now()-time+timing_adjust > rospy.Duration.from_sec(a["end"])): continue if a["type"] == "viseme": #rospy.loginfo("Viseme: " + a["id"]) vgoal = dragon_msgs.msg.VisemeGoal(constant=a["id"]) self.viseme_client.send_goal(vgoal) self.feedback.viseme = a["id"] self.server.publish_feedback(self.feedback) elif a["type"] == "expression": rospy.loginfo("Expression: " + a["id"]) egoal = dragon_msgs.msg.ExpressionMotionGoal(type='expression',constant=a["id"]) self.express_client.send_goal(egoal) self.feedback.action = a["id"] self.server.publish_feedback(self.feedback) elif a["type"] == "motion": rospy.loginfo("Motion: " + str(a["id"])) egoal = dragon_msgs.msg.ExpressionMotionGoal(type='motion',constant=a["id"]) self.express_client.send_goal(egoal) self.feedback.action = a["id"] self.server.publish_feedback(self.feedback) elif a["type"] == "lookat": rospy.loginfo("Lookat: x: " + str(a["x"]) + " y: " + str(a["y"]) + " z: " + str(a["z"])) lgoal = dragon_msgs.msg.LookatGoal(x = a["x"], y = a["y"], z = a["z"]) self.lookat_client.send_goal(lgoal) elif a["type"] == "lookat_frame": rospy.loginfo("Lookat: target: " + a["target"]) try: (trans, rot) = self.tf_listener.lookupTransform('/dragonbot',frame, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn("Error looking up transform.") else: lgoal = dragon_msgs.msg.LookatGoal(x = trans[0]*100, y = trans[1]*100, z = trans[2]*100) #m to cm self.lookat_client.send_goal(lgoal) elif a["type"] == "lookat_off": rospy.loginfo("Lookat off") lgoal = dragon_msgs.msg.LookatGoal(state = "off") self.lookat_client.send_goal(lgoal) elif a["type"] == "track": rospy.loginfo("Track: " + a["target"]) tgoal = dragon_msgs.msg.TrackGoal(on = True, target = a["target"]) self.track_client.send_goal(tgoal) elif a["type"] == "track_off": rospy.loginfo("Tracking off") tgoal = dragon_msgs.msg.TrackGoal(on = False) self.track_client.send_goal(tgoal) elif a["type"] == "eye_open": rospy.loginfo("Eyes open") bgoal = dragon_msgs.msg.BlinkGoal(constant = "STOP") self.blink_client.send_goal(bgoal) elif a["type"] == "eye_close": rospy.loginfo("Eyes closed") bgoal = dragon_msgs.msg.BlinkGoal(constant = "START") self.blink_client.send_goal(bgoal) elif a["type"] == "blink": rospy.loginfo("Blink") bgoal = dragon_msgs.msg.BlinkGoal(constant = "ONCE") self.blink_client.send_goal(bgoal) elif a["type"] == "pose": rospy.loginfo("Pose " + str(a)) igoal = dragon_msgs.msg.IKGoal(vel = a["vel"], acc = a["acc"], x = a["x"], y = a["y"], z = a["z"], theta = a["theta"], neck = a["neck"], state = "on") self.ik_client.send_goal(igoal) elif a["type"] == "pose_off": rospy.loginfo("Pose off") igoal = dragon_msgs.msg.IKGoal(state = "off") self.ik_client.send_goal(igoal) else: rospy.logwarn("action not recognized") if preempted: rospy.loginfo("Preempted") #self.express_client.cancel_all_goals() self.viseme_client.cancel_all_goals() self.sound_client.stopAll() goal = dragon_msgs.msg.LookatGoal(state = "off") self.lookat_client.send_goal(goal) goal = dragon_msgs.msg.TrackGoal(on = False) self.track_client.send_goal(goal) self.server.set_preempted() return else: rospy.loginfo("Waiting for end") while (rospy.Time.now()-time+timing_adjust) < rospy.Duration.from_sec(wave_duration): pass rospy.loginfo("At end -- Success") self.result.result = "SUCCESS" self.viseme_client.cancel_all_goals() #self.express_client.cancel_all_goals() goal = dragon_msgs.msg.LookatGoal(state = "off") self.lookat_client.send_goal(goal) #bgoal = dragon_msgs.msg.BlinkGoal(constant = "STOP") #self.blink_client.send_goal(bgoal) goal = dragon_msgs.msg.TrackGoal(on = False) self.track_client.send_goal(goal) self.server.set_succeeded(self.result) return rospy.logwarn("Speech: the code should probably never reach this point. Setting succeeded anyway.") self.result.result="SUCCESS" self.server.set_succeeded(self.result)
#* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE #* POSSIBILITY OF SUCH DAMAGE. #*********************************************************** # Author: Blaise Gassend import sys if __name__ == '__main__': if len(sys.argv) != 2 or sys.argv[1] == '--help': print 'Usage: %s sound_to_play.(ogg|wav)' % sys.argv[0] print print 'Plays an .OGG or .WAV file. The path to the file should be absolute, and be valid on the computer on which cordial_sound is running.' exit(1) # Import after printing usage for speed. import roslib roslib.load_manifest('cordial_sound') import rospy from cordial_sound.msg import SoundRequest from cordial_sound.libsoundplay import SoundClient rospy.init_node('play', anonymous=True) soundhandle = SoundClient() rospy.sleep(1) print 'Playing "%s".' % sys.argv[1] soundhandle.playWave(sys.argv[1]) rospy.sleep(1)
import roslib; roslib.load_manifest('cordial_sound') import rospy, os, sys from cordial_sound.msg import SoundRequest from cordial_sound.libsoundplay import SoundClient def sleep(t): try: rospy.sleep(t) except: pass if __name__ == '__main__': rospy.init_node('soundplay_test', anonymous = True) soundhandle = SoundClient() rospy.sleep(1) soundhandle.stopAll() print "This script will run continuously until you hit CTRL+C, testing various sound_node sound types." print #print 'Try to play wave files that do not exist.' #soundhandle.playWave('17') #soundhandle.playWave('dummy') #print 'say' #soundhandle.say('Hello world!') #sleep(3)
import sys if __name__ == '__main__': if len(sys.argv) != 2 or sys.argv[1] == '--help': print 'Usage: %s <sound_id>' % sys.argv[0] print print 'Plays one of the built-in sounds based on its integer ID. Look at the cordial_sound/SoundRequest message definition for IDs.' exit(1) # Import here so that usage is fast. import roslib roslib.load_manifest('cordial_sound') import rospy from cordial_sound.msg import SoundRequest from cordial_sound.libsoundplay import SoundClient rospy.init_node('play', anonymous=True) soundhandle = SoundClient() rospy.sleep(1) num = int(sys.argv[1]) print 'Playing sound %i.' % num soundhandle.play(num) rospy.sleep(1)
class FoodChoiceDay2(smach.State): def __init__(self, dm, tm, exp_info, dialogue_info, food_phrases): smach.State.__init__(self, outcomes=['panic', 'next_round', 'end', 'timeout']) self.dm = dm self.tm = tm self.ntimes = 0 self.day, self.lessons = exp_info self.fp = food_phrases[self.day] self.all_foods = self.fp["bad"] + self.fp["good"] + self.fp["sometimes"] self.dialogue = dialogue_info self.gui_prefix = "dragon_GUI/" self.dg = DialogueManager(self.dm, self.tm, self.day + "_foods", self.dialogue) self.sc = SoundClient() self.music_folder = roslib.packages.get_pkg_dir("expeditions_year1")+ "/music/" self.feedback_levels = {i:{"good":0,"bad":0} for i in self.fp["groups"]} self.feedback_levels["all"] = {"good":0, "bad":0} self.feedback_levels["reminders"] = 0 self.prev_items = dict(good = [], bad = [], sometimes = []) self.target_group = "all" self.selected_foods = [] self.sometimes_feedback = {food:False for food in self.fp["sometimes"]} self.exp_start_time = rospy.get_param("~start_time") self.duration = rospy.get_param("~max_time") self.seen_bad = set() def execute(self, userdata): print "===============================================" print "+ FOOD DIALOGUE +" print "-----------------------------------------------" self.exp_start_time = rospy.get_param("~start_time") if rospy.Time.now().secs -self.exp_start_time > self.duration: try: self.dg.play_dialogue("timeout") except PanicException: return 'panic' except NextStateException: return 'end' except NextPhraseException: pass return 'timeout' phrases = self.fp["phrases"] if rospy.get_param("~first_time_foods"): try: self.dg.play_dialogue(phrases["intro"]) except PanicException: return 'panic' except NextStateException: rospy.set_param("~first_time_foods",True) return 'end' except NextPhraseException: pass # reset everything self.target_group = "all" self.feedback_levels["all"]["bad"] = 0 for g in self.fp["groups"].keys(): self.feedback_levels[g]["bad"] = 0 self.feedback_levels["all"]["good"] = 0 for g in self.fp["groups"].keys(): self.feedback_levels[g]["good"] = 0 self.selected_foods = [] gui_name = self.day + "_" + "_".join(self.selected_foods) self.tm.change(gui_name) panicked = False while not rospy.is_shutdown(): resp = self.tm.wait_for_press("dragon_GUI/food_select") if resp == "panic": return 'panic' elif resp == "GO": break elif resp == "reminder": level = self.feedback_levels["reminders"] try: self.dg.play_dialogue(phrases["reminders"][level]) except PanicException: return 'panic' except NextStateException: rospy.set_param("~first_time_foods",True) return 'end' except NextPhraseException: pass if level < len(phrases["reminders"]) - 1: self.feedback_levels["reminders"] += 1 elif resp == "next": rospy.set_param("~first_time_foods",True) return 'end' elif resp == "--": continue elif resp in self.selected_foods: self.selected_foods.remove(resp) else: self.selected_foods.append(resp) self.selected_foods.sort() gui_name = self.day + "_" + "_".join(self.selected_foods) self.tm.change(gui_name) #reset reminder feedback level self.feedback_levels["reminders"] = 0 # have a bunch of choices in self.selected_foods bad_foods = [] good_foods = [] sometimes_foods = [] for food in self.selected_foods: if food in self.fp["bad"]: bad_foods.append(food) elif food in self.fp["good"]: good_foods.append(food) elif food in self.fp["sometimes"]: sometimes_foods.append(food) rospy.loginfo("Selected foods:") rospy.loginfo(" Good foods: " + ",".join(good_foods)) rospy.loginfo(" Bad foods: " + ",".join(bad_foods)) rospy.loginfo(" Sometimes foods: " + ",".join(sometimes_foods)) prev_items = self.prev_items["bad"] + self.prev_items["good"] + self.prev_items["sometimes"] added_items = filter(lambda f: f not in prev_items, self.selected_foods) removed_items = filter(lambda f: f not in self.selected_foods, prev_items) # escalate level only if failed to improve from last time # failure to improve: there were bad items before and we didn't # remove one of the ones from the group we were targeting for # improvement or there were missing good foods and we didn't add # one of them from the group we were targeting had_bad = len(self.prev_items["bad"]) > 0 lacked_good = not set(self.prev_items["good"]) == set(self.fp["good"]) and not rospy.get_param("~first_time_foods") rospy.loginfo("Added foods: " + str(added_items) + " Removed foods: " + str(removed_items)) rospy.loginfo("Target group: " + self.target_group) if self.target_group == "all": removed_target_bad = len(set(removed_items) & set(self.fp["bad"])) > 0 added_target_good = len(set(added_items) & set(self.fp["good"])) > 0 else: removed_target_bad = len(set(removed_items) & set(self.fp["bad"]) & set(self.fp["groups"][self.target_group])) > 0 added_target_good = len(set(added_items) & set(self.fp["good"]) & set(self.fp["groups"][self.target_group])) > 0 #reset target group and feedback levels if we're switching from #removing bad items to adding good items and vice versa if had_bad and len(bad_foods) == 0: self.target_group = "all" self.feedback_levels["all"]["bad"] = 0 for g in self.fp["groups"].keys(): self.feedback_levels[g]["bad"] = 0 if not had_bad and len(bad_foods) > 0: self.target_group = "all" # reset good food feedback level self.feedback_levels["all"]["good"] = 0 for g in self.fp["groups"].keys(): self.feedback_levels[g]["good"] = 0 print "Seen the following bad foods: " + str(list(self.seen_bad)) replaced_bad = len(self.seen_bad & set(bad_foods)) > 0 #escalate if there was a bad item and no bad item was removed, #or if the kid added a bad item they've seen before if (had_bad and not removed_target_bad) or replaced_bad: if self.target_group == "all": self.feedback_levels["all"]["bad"] += 1 else: for g in self.fp["groups"].keys(): if self.target_group == g: self.feedback_levels[g]["bad"] += 1 #only adjust good feedback level if no bad foods left if lacked_good and not added_target_good and len(bad_foods) == 0: if self.target_group == "all": self.feedback_levels["all"]["good"] += 1 else: for g in self.fp["groups"].keys(): if self.target_group == g: self.feedback_levels[g]["good"] += 1 self.first_round = False try: self.dg.play_dialogue(phrases["plate_chant"]) except PanicException: return 'panic' except NextStateException: rospy.set_param("~first_time_foods",True) return 'end' except NextPhraseException: pass rospy.loginfo("Current feedback levels: " + str(self.feedback_levels)) if not len(bad_foods) == 0: rospy.loginfo("BZZZZT") self.sc.playWave(self.music_folder + "bzzt.wav") rospy.sleep(2.0) # if the target group is specific and doesn't have any more bad items, switch targets if not self.target_group == "all" and len(set(bad_foods) & set(self.fp["groups"][self.target_group])) == 0: rospy.loginfo("Switching target groups...") bad_groups = [] for g in self.fp["groups"].keys(): if len(set(bad_foods) & set(self.fp["groups"][g])) > 0: bad_groups.append(g) self.target_group = random.choice(bad_groups) rospy.loginfo("Switched to group: " + self.target_group) #figure out the correct feedback phrase feedback_phrase = "" if self.target_group == "all" and self.feedback_levels[self.target_group]["bad"] == -1: feedback_phrase = "" # if we're past the end of our lists elif self.feedback_levels[self.target_group]["bad"] > len(self.fp["phrases"]["has_bad"][self.target_group])-1: if self.target_group == "all": bad_groups = [] for g in self.fp["groups"].keys(): if len(set(bad_foods) & set(self.fp["groups"][g])) > 0: bad_groups.append(g) self.target_group = random.choice(bad_groups) feedback_phrase = self.fp["phrases"]["has_bad"][self.target_group][self.feedback_levels[self.target_group]["bad"]] else: #give specific feedback food_options = list(set(bad_foods) & set(self.fp["groups"][self.target_group])) food_target = random.choice(food_options) level = min(len(self.fp["phrases"]["specific"][food_target])-1, self.feedback_levels[self.target_group]["bad"]-len(self.fp["phrases"]["has_bad"][self.target_group])) feedback_phrase = self.fp["phrases"]["specific"][food_target][level] else: feedback_phrase = self.fp["phrases"]["has_bad"][self.target_group][self.feedback_levels[self.target_group]["bad"]] if len(feedback_phrase) > 0: rospy.loginfo("Playing feedback: " + str(feedback_phrase)) try: self.dg.play_dialogue(feedback_phrase) except PanicException: return 'panic' except NextStateException: rospy.set_param("~first_time_foods",True) return 'end' except NextPhraseException: pass elif not set(good_foods) == set(self.fp["good"]): rospy.loginfo("ding") self.sc.playWave(self.music_folder + "ding.wav") rospy.sleep(1.0) if not len(filter(lambda f: not self.sometimes_feedback[f], sometimes_foods)) == 0: target_food = random.choice(filter(lambda f: not self.sometimes_feedback[f], sometimes_foods)) feedback_phrase = self.fp["phrases"]["specific"][target_food][0] rospy.loginfo("Playing sometimes feedback for food: " + str(target_food)) try: self.dg.play_dialogue(feedback_phrase) self.sometimes_feedback[target_food] = True except PanicException: return 'panic' except NextStateException: rospy.set_param("~first_time_foods",True) return 'end' except NextPhraseException: pass # if there's a specific target group and it's not missing any more good items, switch targets if not self.target_group == "all": target_good = set(self.fp["groups"][self.target_group]) & set(self.fp["good"]) # if the target group isn't missing any more good items if target_good == (set(good_foods) & set(self.fp["groups"][self.target_group])): rospy.loginfo("Switching target groups...") good_groups = [] for g in self.fp["groups"].keys(): group_good = set(self.fp["groups"][g]) & set(self.fp["good"]) if not group_good == (set(good_foods) & set(self.fp["groups"][g])): good_groups.append(g) self.target_group = random.choice(good_groups) rospy.loginfo("Switched to group: " + self.target_group) #figure out the correct feedback phrase feedback_phrase = "" if self.target_group == "all" and self.feedback_levels[self.target_group]["bad"] == -1: feedback_phrase = "" # if we're past the end of our lists elif self.feedback_levels[self.target_group]["good"] > len(self.fp["phrases"]["missing_good"][self.target_group])-1: if self.target_group == "all": good_groups = [] for g in self.fp["groups"].keys(): group_good = set(self.fp["groups"][g]) & set(self.fp["good"]) if not group_good.issubset(set(good_foods)): good_groups.append(g) self.target_group = random.choice(good_groups) feedback_phrase = self.fp["phrases"]["missing_good"][self.target_group][self.feedback_levels[self.target_group]["good"]] else: #give specific feedback food_options = list((set(self.fp["groups"][self.target_group]) & set(self.fp["good"]))-set(good_foods)) food_target = random.choice(food_options) level = min(len(self.fp["phrases"]["specific"][food_target])-1, self.feedback_levels[self.target_group]["good"]-len(self.fp["phrases"]["missing_good"][self.target_group])) feedback_phrase = self.fp["phrases"]["specific"][food_target][level] else: feedback_phrase = self.fp["phrases"]["missing_good"][self.target_group][self.feedback_levels[self.target_group]["good"]] if len(feedback_phrase) > 0: rospy.loginfo("Playing feedback: " + str(feedback_phrase)) try: self.dg.play_dialogue(feedback_phrase) except PanicException: return 'panic' except NextStateException: rospy.set_param("~first_time_foods",True) return 'end' except NextPhraseException: pass else: rospy.loginfo("FANFARE! YAY!") self.sc.playWave(self.music_folder + "fanfare.wav") rospy.sleep(2.0) try: self.dg.play_dialogue(phrases["success"]) except PanicException: return 'panic' except NextStateException: rospy.set_param("~first_time_foods",True) return 'end' except NextPhraseException: pass rospy.set_param("~first_time_foods",True) return 'end' self.prev_items["good"] = good_foods self.prev_items["bad"] = bad_foods self.prev_items["sometimes"] = sometimes_foods self.seen_bad = self.seen_bad | set(bad_foods) rospy.set_param("~first_time_foods", False) return 'next_round'
class Workout(smach.State): def __init__(self, dm, tm, info, dialogue_info): smach.State.__init__(self, outcomes=['continue', 'end', 'timeout', 'panic']) self.sc = SoundClient() self.dm = dm self.tm = tm self.exp_start = rospy.get_param("~start_time") self.duration = rospy.get_param("~max_time") self.break_time = rospy.Duration(60) self.encouragement_every = 10 self.dialogue = dialogue_info #self.workout_phrases = workout_info self.day, self.lesson = info self.gui_prefix = "dragon_GUI/" self.seen_victory = False self.music_folder = roslib.packages.get_pkg_dir("expeditions_year1")+ "/music/" #pose is: x, y, z, (theta, neck, vel, acc [optional]) self.poses = {'right': (0, 2.4, .5), 'left': (0, -2.4, .5), 'up': (0, 0, 3), 'down': (0, 0, 0)} # song and bpm self.songs = {'mario_yoshi.wav':104, 'Donkey_Kong_Country_Jungle_Stomp_OC_ReMix.wav':84, 'Legend_of_Zelda_A_Link_to_the_Past_Kakariko_Rave_Party_OC_ReMix.wav':160, 'Legend_of_Zelda_Ocarina_of_Time_This_Valley_Rocks_OC_ReMix.wav':96, 'Super_Mario_World_2_Yoshi\'s_Island_Dino_Band_Rehearsal_OC_ReMix.wav':112, 'Super_Mario_World_Swanky_Vegas_OC_ReMix.wav':120} self.current_song = "" self.dg = DialogueManager(self.dm, self.tm, self.day + "_workout", self.dialogue) self.vol = 0.3 def do_victory(self): print "DOING VICTORY DANCE RAWR" v = 1 a = .08 routine = ['up','down','up','down','up'] self.dm.pose_off() self.dm.pose(0,0,0,vel=v, acc=a) if not self.seen_victory: try: self.dg.play_dialogue("victory_dance") except NextPhraseException: pass for move in routine: m = self.poses[move] self.dm.pose(x = m[0], y = m[1], z =m[2], vel = v, acc = a) rospy.sleep(1.0) self.dm.pose_off() try: self.dg.play_dialogue("what_do_you_think") except NextPhraseException: pass self.seen_victory = True def bpm_adjust(self, bpm): while bpm > 70: bpm = bpm/2 return bpm def execute(self, userdata): self.seen_victory=False print "===============================================" print "+ WORKOUT GAME +" print "-----------------------------------------------" try: #TODO: play correct outro statement self.dg.play_dialogue("intro_dialogue") except PanicException: return 'panic' except NextStateException: return 'end' except NextPhraseException: pass routine = ['up','down'] self.current_song, bpm = random.choice(self.songs.items()) i = 0 v = 1 a = 0.5 bpm = self.bpm_adjust(bpm) time_adjust = rospy.Duration(0.0) delay_adjust = rospy.Duration(0.0) move_sleep = rospy.Duration(60/bpm) self.dm.pose(0,0,0) #play workout routine #below for timeout from beginning of experiment #start = rospy.get_param("~start_time") #below for timeout from beginning of workout game start = rospy.Time.now().secs self.sc.playWave(self.music_folder + self.current_song) self.sc.waveVol(self.music_folder + self.current_song, self.vol) try: self.dg.play_dialogue("post_music", interrupt=False) except PanicException: self.dm.pose_off() self.sc.stopAll() return 'panic' except NextStateException: self.dm.pose_off() self.sc.stopAll() return 'end' except NextPhraseException: pass self.tm.change("stopped_dancing") rospy.sleep(delay_adjust) next_break = rospy.Time.now() + self.break_time nbreaks = 1 rospy.loginfo("Next break: " + str(next_break.secs)) dance_start = rospy.Time.now() not_dancing_counter = 0 print "Time elapsed (game): " + str((rospy.Time.now()-dance_start).secs) print "Time elapsed (experiment): " + str(rospy.Time.now().secs-start) while rospy.Time.now().secs-start < self.duration and not rospy.is_shutdown(): print "Time elapsed (game): " + str((rospy.Time.now()-dance_start).secs) print "Time elapsed (experiment): " + str(rospy.Time.now().secs-start) print "Time until next break: " + str((next_break - rospy.Time.now()).secs) if rospy.Time.now() > next_break: if nbreaks == 1: break_phrase = "new_song" elif nbreaks == 2: break_phrase = "break1" elif nbreaks == 3: break_phrase = "break2" else: break_phrase = "finished_dancing" nbreaks = nbreaks + 1 self.sc.stopAll() try: resp = self.dg.play_dialogue(break_phrase, interrupt = True) except PanicException: self.dm.pose_off() self.sc.stopAll() return 'panic' except NextStateException: self.dm.pose_off() self.sc.stopAll() return 'end' except NextPhraseException: pass if "yes_change" in resp: song, bpm = random.choice(self.songs.items()) while song == self.current_song: #note that this will break if there's only one song song, bpm = random.choice(self.songs.items()) self.current_song = song bpm = self.bpm_adjust(bpm) move_sleep = rospy.Duration(60/bpm) self.sc.playWave(self.music_folder + self.current_song) self.sc.waveVol(self.music_folder + self.current_song, self.vol) next_break = rospy.Time.now() + self.break_time rospy.loginfo("Next break: " + str(next_break.secs)) self.tm.change("stopped_dancing") continue if "yes_break1" in resp or "yes_break2" in resp and not "hum" in resp: try: self.do_victory() except PanicException: self.dm.pose_off() self.sc.stopAll() return 'panic' except NextStateException: self.dm.pose_off() self.sc.stopAll() return 'end' except NextPhraseException: pass if break_phrase == "finished_dancing" and not "no_finished" in resp: break self.sc.playWave(self.music_folder + self.current_song) self.sc.waveVol(self.music_folder + self.current_song, self.vol) next_break = rospy.Time.now() + self.break_time rospy.loginfo("Next break: " + str(next_break.secs)) self.tm.change("stopped_dancing") continue p = self.tm.last_press(self.gui_prefix + "stopped_dancing") if p == "next": self.dm.pose_off() self.sc.stopAll() return "end" elif p == "panic": self.dm.pose_off() self.sc.stopAll() return "panic" elif p == "music_up": self.vol = self.vol + 0.1 if self.vol > 1.0: rospy.loginfo("At max volume") self.vol = 1.0 self.sc.waveVol(self.music_folder + self.current_song, self.vol) self.tm.change("stopped_dancing") continue elif p == "music_down": self.vol = self.vol - 0.1 if self.vol < 0.0: rospy.loginfo("At min volume") self.vol = 0.0 self.sc.waveVol(self.music_folder + self.current_song, self.vol) self.tm.change("stopped_dancing") continue elif p == "music_stopped": self.sc.stopAll() song, bpm = random.choice(self.songs.items()) while song == self.current_song: #note that this will break if there's only one song song, bpm = random.choice(self.songs.items()) self.current_song = song bpm = self.bpm_adjust(bpm) move_sleep = rospy.Duration(60/bpm) self.sc.playWave(self.music_folder + self.current_song) self.sc.waveVol(self.music_folder + self.current_song, self.vol) self.tm.change("stopped_dancing") continue elif p == "never_dancing": not_dancing_counter += 1 if not_dancing_counter > 4: not_dancing_counter = 4 try: resp = self.dg.play_dialogue("not_dancing" + str(not_dancing_counter)) except PanicException: self.dm.pose_off() self.sc.stopAll() return 'panic' except NextStateException: self.dm.pose_off() self.sc.stopAll() return 'end' except NextPhraseException: pass if "yes_finished" in resp: break self.sc.playWave(self.music_folder + self.current_song) self.sc.waveVol(self.music_folder + self.current_song, self.vol) self.tm.change("stopped_dancing") continue elif p == "stopped_dancing": self.sc.stopAll() try: resp = self.dg.play_dialogue("stop_dancing", interrupt = True) except PanicException: self.dm.pose_off() self.sc.stopAll() return 'panic' except NextStateException: self.dm.pose_off() self.sc.stopAll() return 'end' except NextPhraseException: pass if "dance_more" in resp: self.sc.playWave(self.music_folder + self.current_song) self.sc.waveVol(self.music_folder + self.current_song, self.vol) self.tm.change("stopped_dancing") continue if "yes_change" in resp: song, bpm = random.choice(self.songs.items()) while song == self.current_song: #note that this will break if there's only one song song, bpm = random.choice(self.songs.items()) self.current_song = song bpm = self.bpm_adjust(bpm) move_sleep = rospy.Duration(60/bpm) self.sc.playWave(self.music_folder + self.current_song) self.sc.waveVol(self.music_folder + self.current_song, self.vol) self.tm.change("stopped_dancing") continue if "no_change" in resp: break_phrase = "finished_dancing" self.sc.stopAll() try: resp = self.dg.play_dialogue(break_phrase, interrupt = True) except PanicException: self.dm.pose_off() self.sc.stopAll() return 'panic' except NextStateException: self.dm.pose_off() self.sc.stopAll() return 'end' except NextPhraseException: break print resp if not "no_finished" in resp: break else: self.sc.playWave(self.music_folder + self.current_song) self.sc.waveVol(self.music_folder + self.current_song, self.vol) self.tm.change("stopped_dancing") continue move = i % len(routine) m = self.poses[routine[move]] self.dm.pose(x = m[0], y = m[1], z =m[2], vel = v, acc = a) i = i + 1 if i % self.encouragement_every == 0: try: self.dg.play_dialogue("energized_comment", interrupt = False) except PanicException: self.dm.pose_off() self.sc.stopAll() return 'panic' except NextStateException: self.dm.pose_off() self.sc.stopAll() return 'end' except NextPhraseException: pass rospy.sleep(move_sleep-time_adjust) if rospy.Time.now().secs-start >= self.duration: print "TIMED OUT" rospy.sleep(2.0) try: self.dg.play_dialogue("timeout") except PanicException: self.dm.pose_off() self.sc.stopAll() return 'panic' except NextStateException: self.dm.pose_off() self.sc.stopAll() return 'end' except NextPhraseException: pass if not self.seen_victory: try: self.do_victory() except PanicException: self.dm.pose_off() self.sc.stopAll() return 'panic' except NextStateException: self.dm.pose_off() self.sc.stopAll() return 'end' except NextPhraseException: pass self.dm.pose_off() self.sc.stopAll() return 'timeout' else: if not self.seen_victory: try: self.do_victory() except PanicException: self.dm.pose_off() self.sc.stopAll() return 'panic' except NextStateException: self.dm.pose_off() self.sc.stopAll() return 'end' except NextPhraseException: pass self.dm.pose_off() self.sc.stopAll() return "end"
import rospy, os, sys from cordial_sound.msg import SoundRequest from cordial_sound.libsoundplay import SoundClient def sleep(t): try: rospy.sleep(t) except: pass if __name__ == '__main__': rospy.init_node('soundplay_test', anonymous=True) soundhandle = SoundClient() rospy.sleep(1) soundhandle.stopAll() print "This script will run continuously until you hit CTRL+C, testing various sound_node sound types." print #print 'Try to play wave files that do not exist.' #soundhandle.playWave('17') #soundhandle.playWave('dummy') #print 'say' #soundhandle.say('Hello world!') #sleep(3)
class PlayerServer(): _feedback = PlayerFeedback() _result = PlayerResult() def __init__(self, phone_face, delay=0.0, phrase_file=None, use_tts=False, voice=None): self._use_tts = use_tts in ['true', 'True', 'TRUE', '1'] if phrase_file and not len(phrase_file) > 0 and not use_tts: rospy.logerr( "CoRDial Player Error: Must specify phrase file or allow tts! Continuing without sound..." ) self._sound = False elif not phrase_file and not use_tts: rospy.logerr( "CoRDial Player Error: Must specify phrase file or allow tts! Continuing without sound..." ) self._sound = False # elif (not phrase_file or len(phrase_file)>0) and use_tts and (not ivona_secret_key or not ivona_access_key): # rospy.logerr("CoRDial Player Error: Must specify ivona keys to allow tts! Continuing without sound...") # self._sound = False # self._use_tts=False else: self._sound = True self._speech_delay_time = delay self._phone_face = phone_face self._phrases = None if phrase_file and len(phrase_file) > 0: self._sound_client = SoundClient() rospy.sleep(0.5) self._sound_client.stopAll() rospy.loginfo( "CoRDial Player reading Phrase File... this could take a while" ) with open(phrase_file, 'r') as f: s = f.read() self._phrases = yaml.load(s) rospy.loginfo("Phrase file read.") if self._use_tts: self._tts = CoRDialTTS(voice) base_topic = "" self._behavior_client = actionlib.SimpleActionClient( base_topic + 'Behavior', BehaviorAction) if self._phone_face: self._face_pub = rospy.Publisher(base_topic + 'face', FaceRequest, queue_size=10) rospy.sleep(0.5) rospy.loginfo("CoRdial Player waiting for behavior server..") self._behavior_client.wait_for_server() rospy.loginfo("CoRDial Player connected to behavior server") self._feedback.behavior = "none" rospy.loginfo("Starting CoRDial Player server...") self._server = actionlib.SimpleActionServer(base_topic + 'Player', PlayerAction, execute_cb=self.execute_cb, auto_start=False) info = "" if self._phone_face: info += ", using CoRDial face" else: info += ", NOT using CoRDial face" if self._use_tts: info += " and using TTS with voice " + voice elif self._phrases: info += " and NOT using TTS" if self._phrases: info += "; phrase file is " + phrase_file if not self._use_tts and not self._phrases: info += " and with NO sound" info += ". Delay time is " + str(self._speech_delay_time) + "s." self._server.start() rospy.loginfo("CoRDial Player server started" + info) def execute_cb(self, goal): rospy.loginfo("Phrase playing: " + str(goal.phrase)) preempted = False if goal.interrupt == True: rospy.loginfo("Stopping audio") if self._phrases: self._sound_client.stopAll() if self._use_tts: self._tts.shutup() #TODO: more intelligent goal cancelling? rospy.loginfo("Stopping behaviors") self._behavior_client.cancel_all_goals() phrase_found = False if self._phrases: try: behaviors = self._phrases[goal.phrase]["behaviors"] phrase_found = True except KeyError as k: if not self._use_tts: rospy.logerr("Phrase id %s not recognized" % goal.phrase) rospy.logerr(str(type(k))) rospy.logerr(str(k.args)) self._result.result = "FAILED" self._server.set_aborted(self._result) return else: phrase_found = False rospy.logwarn("Phrase id %s not recognized; using TTS" % goal.phrase) if not phrase_found: time1 = rospy.Time.now() phrase, behaviors = self._tts.extract_behaviors(goal.phrase) if self._phone_face: # visemes = ['AO_AW', 'CH_SH_ZH', 'R_ER', 'L', 'IDLE', 'AA_AH', 'EY', 'M_B_P', 'N_NG_D_Z', 'EH_AE_AY', 'OO', 'F_V'] visemes = [ "BILABIAL", "LABIODENTAL", "INTERDENTAL", "DENTAL_ALVEOLAR", "POSTALVEOLAR", "VELAR_GLOTTAL", "CLOSE_FRONT_VOWEL", "OPEN_FRONT_VOWEL", "MID_CENTRAL_VOWEL", "OPEN_BACK_VOWEL", "CLOSE_BACK_VOWEL", 'IDLE' ] viseme_behaviors = filter(lambda b: b["id"] in visemes, behaviors) behaviors = filter(lambda b: b["id"] not in visemes, behaviors) min_duration = 0.05 for i in range(0, len(viseme_behaviors) - 1): viseme_behaviors[i]["duration"] = viseme_behaviors[ i + 1]["start"] - viseme_behaviors[i]["start"] viseme_behaviors[-1]["duration"] = min_duration viseme_behaviors = filter(lambda b: b["duration"] >= min_duration, viseme_behaviors) ordered_visemes = sorted(viseme_behaviors, key=lambda b: b["start"]) viseme_ids = map(lambda b: b["id"], ordered_visemes) viseme_times = map(lambda b: b["start"], ordered_visemes) viseme_speed = 10 # viseme_speed = map(lambda b: b["duration"], ordered_visemes) viseme_req = FaceRequest(visemes=viseme_ids, viseme_ms=viseme_speed, times=viseme_times) ordered_behaviors = sorted(behaviors, key=lambda behavior: behavior["start"]) speech_delay_time = self._speech_delay_time #for behaviors if speech_delay_time < 0: timing_adjust = rospy.Duration.from_sec(0 - speech_delay_time) speech_delay_time = 0 else: timing_adjust = rospy.Duration.from_sec(0) self.speech_duration = 0.0 self.speech_start_time = rospy.Time.now() if phrase_found: wave_file = self._phrases[goal.phrase]["file"] with contextlib.closing(wave.open(wave_file, 'r')) as f: frames = f.getnframes() rate = f.getframerate() self.speech_duration = frames / float(rate) self.speech_start_time = rospy.Time.now() def speak(): self._sound_client.playWave(wave_file) rospy.loginfo("Speech: playing wave file -- duration: " + str(self.speech_duration)) t = Timer(speech_delay_time, speak) t.start() else: def speak(): ogg_file = self._tts.say(phrase) self.speech_duration = 3.0 self.speech_start_time = rospy.Time.now() rospy.loginfo("Speech: playing tts speech -- duration: " + str(self.speech_duration)) self._sound_client.playWave(ogg_file) t = Timer(speech_delay_time, speak) t.start() if self._phone_face: viseme_delay_time = timing_adjust.to_sec() def send_visemes(): self._face_pub.publish(viseme_req) t = Timer(viseme_delay_time, send_visemes) t.start() start_time = rospy.Time.now() for a in ordered_behaviors: rospy.loginfo("Playing behavior: " + str(a)) while rospy.Time.now() - start_time < rospy.Duration.from_sec( a["start"] ) + timing_adjust and not self._server.is_preempt_requested(): pass if self._server.is_preempt_requested(): preempted = True break if_conflict = BehaviorGoal.DROP if "interrupt" in a.keys(): if_conflict = BehaviorGoal.OVERRIDE if "args" in a.keys(): args = a["args"] else: args = [] if "hold" in a.keys(): hold = bool(a["hold"]) else: hold = True # default is yes, hold pose until told otherwise if "move_time" in a.keys(): end_move = rospy.Time.now() + rospy.Duration.from_sec( a["move_time"]) else: end_move = rospy.Time() if "end" in a.keys(): end_hold = rospy.Duration.from_sec(a["end"]) + start_time else: end_hold = rospy.Time() goal = BehaviorGoal(behavior=a["id"], end_hold=end_hold, end_move=end_move, return_to_prior=not hold, if_conflict=if_conflict, args=args, wait_and_block=False) #old version self._behavior_client.send_goal(goal) #old version self._feedback.behavior = a["id"] self._server.publish_feedback(self._feedback) if preempted: rospy.loginfo("Preempted") self._behavior_client.cancel_all_goals() if phrase_found: self._sound_client.stopAll() else: self._tts.shutup() self._server.set_preempted() return else: rospy.loginfo("Waiting for end") while (rospy.Time.now() - self.speech_start_time) < rospy.Duration.from_sec( self.speech_duration) + rospy.Duration.from_sec( speech_delay_time): pass rospy.loginfo("At end -- Success") self._result.result = PlayerResult.DONE self._server.set_succeeded(self._result) return rospy.logwarn( "Speech: the code should probably never reach this point. Setting succeeded anyway." ) self._result.result = PlayerResult.DONE self._server.set_succeeded(self._result)
def __init__(self, phone_face, delay=0.0, phrase_file=None, use_tts=False, voice=None): self._use_tts = use_tts in ['true', 'True', 'TRUE', '1'] if phrase_file and not len(phrase_file) > 0 and not use_tts: rospy.logerr( "CoRDial Player Error: Must specify phrase file or allow tts! Continuing without sound..." ) self._sound = False elif not phrase_file and not use_tts: rospy.logerr( "CoRDial Player Error: Must specify phrase file or allow tts! Continuing without sound..." ) self._sound = False # elif (not phrase_file or len(phrase_file)>0) and use_tts and (not ivona_secret_key or not ivona_access_key): # rospy.logerr("CoRDial Player Error: Must specify ivona keys to allow tts! Continuing without sound...") # self._sound = False # self._use_tts=False else: self._sound = True self._speech_delay_time = delay self._phone_face = phone_face self._phrases = None if phrase_file and len(phrase_file) > 0: self._sound_client = SoundClient() rospy.sleep(0.5) self._sound_client.stopAll() rospy.loginfo( "CoRDial Player reading Phrase File... this could take a while" ) with open(phrase_file, 'r') as f: s = f.read() self._phrases = yaml.load(s) rospy.loginfo("Phrase file read.") if self._use_tts: self._tts = CoRDialTTS(voice) base_topic = "" self._behavior_client = actionlib.SimpleActionClient( base_topic + 'Behavior', BehaviorAction) if self._phone_face: self._face_pub = rospy.Publisher(base_topic + 'face', FaceRequest, queue_size=10) rospy.sleep(0.5) rospy.loginfo("CoRdial Player waiting for behavior server..") self._behavior_client.wait_for_server() rospy.loginfo("CoRDial Player connected to behavior server") self._feedback.behavior = "none" rospy.loginfo("Starting CoRDial Player server...") self._server = actionlib.SimpleActionServer(base_topic + 'Player', PlayerAction, execute_cb=self.execute_cb, auto_start=False) info = "" if self._phone_face: info += ", using CoRDial face" else: info += ", NOT using CoRDial face" if self._use_tts: info += " and using TTS with voice " + voice elif self._phrases: info += " and NOT using TTS" if self._phrases: info += "; phrase file is " + phrase_file if not self._use_tts and not self._phrases: info += " and with NO sound" info += ". Delay time is " + str(self._speech_delay_time) + "s." self._server.start() rospy.loginfo("CoRDial Player server started" + info)
# * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # * POSSIBILITY OF SUCH DAMAGE. # *********************************************************** # Author: Blaise Gassend import roslib roslib.load_manifest("cordial_sound") import rospy from cordial_sound.msg import SoundRequest from cordial_sound.libsoundplay import SoundClient if __name__ == "__main__": rospy.init_node("shutup", anonymous=True) soundhandle = SoundClient() rospy.sleep(0.5) # let ROS get started... print "Sending stopAll commande every 100 ms." print "Note: This will not prevent a node that is continuing to issue commands" print "from producing sound." print "Press Ctrl+C to exit." while not rospy.is_shutdown(): soundhandle.stopAll() try: rospy.sleep(0.1) except: pass