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"
