def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
output_keys=[],
input_keys=[])
with self:
#maybe i will have to learn again
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={
'succeeded': 'look_wave',
'aborted': 'aborted',
'preempted': 'preempted'
})
#maybe i will have to learn again
smach.StateMachine.add('look_wave',
WaveDetection(),
transitions={
'succeeded': 'say_hello',
'aborted': 'look_wave',
'preempted': 'preempted'
})
smach.StateMachine.add('say_hello',
text_to_say(Say, wait=False),
transitions={
'succeeded': 'wait_state',
'aborted': 'wait_state'
})
smach.StateMachine.add('play_motion_state',
play_motion_sm('wave', skip_planning=True),
transitions={
'succeeded': 'wait_state',
'preempted': 'wait_state',
'aborted': 'wait_state'
})
smach.StateMachine.add('wait_state',
waitstate(),
transitions={
'succeeded': 'look_wave',
'aborted': 'look_wave',
'preempted': 'look_wave'
})
def __init__(self, head_position=None):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted', 'end_searching'],
input_keys=[],
output_keys=['wave_position', 'wave_yaw_degree', 'standard_error'])
with self:
self.userdata.loop_iterations = 0
self.userdata.wave_position = None
self.userdata.wave_yaw_degree = None
smach.StateMachine.add('Move_head_prepare',
prepare_move_head(),
transitions={
'succeeded': 'move_head',
'aborted': 'aborted',
'preempted': 'preempted',
'end_searching': 'end_searching'
})
smach.StateMachine.add('move_head',
move_head_form(head_up_down='normal'),
transitions={
'succeeded': 'Say_Searching',
'preempted': 'Say_Searching',
'aborted': 'aborted'
})
smach.StateMachine.add('Say_Searching',
text_to_say("I am searching, let's see"),
transitions={
'succeeded': 'wave_recognition',
'aborted': 'wave_recognition',
'preempted': 'wave_recognition'
})
smach.StateMachine.add('wave_recognition',
WaveDetection(),
transitions={
'succeeded': 'succeeded',
'aborted': 'Move_head_prepare',
'preempted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(
self, ['succeeded', 'preempted', 'aborted'],
output_keys=['person_location', 'person_location_coord'])
with self:
self.userdata.emergency_location = []
# Some dummy TTS stuff
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
smach.StateMachine.add(
'Prepare_Say_Searching',
prepare_tts('Where are you? Give me signals, please.'),
transitions={
'succeeded': 'Say_Search',
'aborted': 'Say_Search',
'preempted': 'Say_Search'
})
smach.StateMachine.add('Say_Search',
text_to_say(),
transitions={
'succeeded': 'Gesture_Recognition',
'aborted': 'Gesture_Recognition',
'preempted': 'Gesture_Recognition'
})
# Search for a Wave Gesture
# Output_keys: gesture_detected: type Gesture
self.userdata.nav_to_coord = [0, 0, 0]
#Look Down
#Move head right/left
smach.StateMachine.add('Gesture_Recognition',
WaveDetection(),
transitions={
'succeeded': 'Say_Search',
'aborted': 'Gesture_Recognition',
'preempted': 'Gesture_Recognition'
})
smach.StateMachine.add('Say_Search',
text_to_say('Oh! I have found you.'),
transitions={
'succeeded': 'Prepare_Go_To_Wave',
'aborted': 'Prepare_Go_To_Wave',
'preempted': 'Say_Search'
})
smach.StateMachine.add('Prepare_Go_To_Wave',
prepare_go_to_wave(),
transitions={
'succeeded': 'Say_Go_to_Wave',
'aborted': 'Say_Go_to_Wave',
'preempted': 'Say_Go_to_Wave'
})
smach.StateMachine.add('Say_Go_to_Wave',
text_to_say("I'm coming!"),
transitions={
'succeeded': 'Go_to_Wave',
'aborted': 'Gesture_Recognition',
'preempted': 'Gesture_Recognition'
})
smach.StateMachine.add('Go_to_Wave',
nav_to_coord('/base_link'),
transitions={
'succeeded': 'Register_Position',
'aborted': 'Go_to_Wave',
'preempted': 'Go_to_Wave'
})
smach.StateMachine.add(
'Register_Position',
get_current_robot_pose(),
transitions={
'succeeded': 'TreatPoseForCoord',
'aborted': 'Register_Position',
'preempted': 'Register_Position'
},
remapping={'current_robot_pose': 'person_location'})
smach.StateMachine.add('TreatPoseForCoord',
PoseToArray(),
transitions={
'succeeded': 'succeeded',
'aborted': 'Register_Position',
'preempted': 'Register_Position'
})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.loop_iterations = 0
self.userdata.gesture_name = ''
self.userdata.object_name = ""
self.userdata.manip_time_to_play = 4
# Must we say something to start? "I'm ready" or something
# Must we wait for the spoken order?
smach.StateMachine.add(
'init_cocktail',
text_to_say("Ready for cocktail party"),
transitions={'succeeded': 'wait_for_door', 'aborted': 'wait_for_door'})
# We wait for open door and go inside
smach.StateMachine.add(
'wait_for_door',
EnterRoomSM("party_room"),
transitions={'succeeded': 'say_search_wave', 'aborted': 'aborted', 'preempted': 'preempted'})
# Say Wave recognize
smach.StateMachine.add(
'say_search_wave',
text_to_say("Searching for wave"),
transitions={'succeeded': 'wave_recognition', 'aborted': 'wave_recognition'})
# Gesture recognition -> Is anyone waving?
smach.StateMachine.add(
'wave_recognition',
WaveDetection(),
transitions={'succeeded': 'say_wave_recognize', 'aborted': 'ask_for_person',
'preempted': 'preempted'})
# Say Wave recognize
smach.StateMachine.add(
'say_wave_recognize',
text_to_say("Someone waved to me. I will go there"),
transitions={'succeeded': 'prepare_coord_wave', 'aborted': 'prepare_coord_wave'})
# Prepare the goal to the person that is waving
# TODO: it goes a little far to the person...
smach.StateMachine.add(
'prepare_coord_wave',
prepare_coord_wave(),
transitions={'succeeded': 'go_to_person_wave', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to the person -> we assume that gesture will return the position
smach.StateMachine.add(
'go_to_person_wave',
nav_to_coord('/base_link'),
transitions={'succeeded': 'learning_person', 'aborted': 'go_to_person_wave',
'preempted': 'preempted'})
# Ask for person if it can see anyone
smach.StateMachine.add(
'ask_for_person',
text_to_say("I can't see anyone. Can anyone come to me, please?"),
transitions={'succeeded': 'wait_for_person', 'aborted': 'ask_for_person',
'preempted': 'preempted'})
# Wait for person
smach.StateMachine.add(
'wait_for_person',
detect_face(),
transitions={'succeeded': 'learning_person', 'aborted': 'aborted'})
# Learn Person -> Ask name + Face Recognition
# TODO: Set database
smach.StateMachine.add(
'learning_person',
SaveFaceSM(),
transitions={'succeeded': 'ask_order', 'aborted': 'learning_person',
'preempted': 'preempted'})
# Ask for order
smach.StateMachine.add(
'ask_order',
AskQuestionSM("What would you like to drink?", GRAMMAR_NAME),
transitions={'succeeded': 'process_order', 'aborted': 'ask_order',
'preempted': 'preempted'})
# Process the answer
smach.StateMachine.add(
'process_order',
process_order(),
transitions={'succeeded': 'search_food_order', 'aborted': 'ask_order',
'preempted': 'preempted'})
# Say what he ask
smach.StateMachine.add(
'say_got_it',
text_to_say("I got it!"),
transitions={'succeeded': 'search_food_order', 'aborted': 'ask_order',
'preempted': 'preempted'})
# Search for object information - It says where the object is, go to it and start object recognition
# TODO: Change how to process object - it must go to storage room always
# TODO: Add some messages in search object
smach.StateMachine.add(
'search_food_order',
SearchObjectSM(),
transitions={'succeeded': 'grasp_food_order', 'aborted': 'Grasp_fail_Ask_Person',
'preempted': 'preempted'})
# Say grasp object
smach.StateMachine.add(
'say_grasp_order',
text_to_say("I'm going to grasp the object"),
transitions={'succeeded': 'grasp_food_order', 'aborted': 'Grasp_fail_Ask_Person',
'preempted': 'preempted'})
# Grasp Object
smach.StateMachine.add(
'grasp_food_order',
DummyStateMachine(),
transitions={'succeeded': 'Grasp_fail_Ask_Person', 'aborted': 'aborted',
'preempted': 'preempted'})
# Ask for grasp object
smach.StateMachine.add(
'Grasp_fail_Ask_Person',
ask_give_object_grasping(),
remapping={'object_to_grasp':'object_name'},
transitions={'succeeded':'Rest_arm', 'aborted':'Rest_arm', 'preempted':'Rest_arm'})
smach.StateMachine.add(
'Rest_arm',
play_motion_sm('rest_object_right'),
transitions={'succeeded':'go_to_party', 'aborted':'go_to_party', 'preempted':'go_to_party'})
# Go to the party room
smach.StateMachine.add(
'go_to_party',
nav_to_poi('party_room'),
transitions={'succeeded': 'say_search_person', 'aborted': 'go_to_party',
'preempted': 'preempted'})
# Say search for person
smach.StateMachine.add(
'say_search_person',
text_to_say("I'm going to search the person who ordered me"),
transitions={'succeeded': 'search_for_person', 'aborted': 'search_for_person',
'preempted': 'preempted'})
# Search for person -> He could change his position
smach.StateMachine.add(
'search_for_person',
recognize_face_concurrent(),
transitions={'succeeded': 'say_found_person', 'aborted': 'prepare_ask_for_person_back',
'preempted': 'preempted'})
# Say found the person
smach.StateMachine.add(
'say_found_person',
text_to_say("I found you!"),
transitions={'succeeded': 'prepare_coord_order', 'aborted': 'aborted',
'preempted': 'preempted'})
# Prepare the goal to the person that ask for the order
smach.StateMachine.add(
'prepare_coord_order',
prepare_coord_order(),
transitions={'succeeded': 'go_to_person_order', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to person
smach.StateMachine.add(
'go_to_person_order',
nav_to_coord('/base_link'),
transitions={'succeeded': 'deliver_drink', 'aborted': 'aborted',
'preempted': 'preempted'})
# Ask for person if it can see anyone
smach.StateMachine.add(
'prepare_ask_for_person_back',
prepare_ask_person_back(),
transitions={'succeeded': 'ask_for_person_back', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'ask_for_person_back',
text_to_say(),
transitions={'succeeded': 'deliver_drink', 'aborted': 'aborted', 'preempted': 'preempted'})
# Deliver Drink
smach.StateMachine.add(
'deliver_drink',
text_to_say("I'm going to deliver the drink"),
transitions={'succeeded': 'Give_Object', 'aborted': 'Give_Object',
'preempted': 'preempted'})
smach.StateMachine.add(
'Give_Object',
give_object(),
transitions={'succeeded':'check_loop', 'aborted':'Give_Object', 'preempted':'Give_Object'})
# End of loop?
smach.StateMachine.add(
'check_loop',
checkLoop(),
transitions={'succeeded': 'wave_recognition', 'aborted': 'aborted',
'preempted': 'preempted', 'end':'leaving_arena'})
# Leaving the arena
smach.StateMachine.add(
'leaving_arena',
nav_to_poi('leave_arena'),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})
def __init__(self,
head_position=None,
text_for_wave_searching="I am looking for you."):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted', 'end_searching'],
input_keys=[],
output_keys=['wave_position', 'wave_yaw_degree', 'standard_error'])
with self:
self.userdata.loop_iterations = 0
self.userdata.wave_position = None
self.userdata.wave_yaw_degree = None
self.userdata.standard_error = ''
smach.StateMachine.add('Move_head_prepare',
prepare_move_head(head_position),
transitions={
'succeeded': 'Concurrence',
'aborted': 'aborted',
'preempted': 'preempted',
'end_searching': 'end_searching'
})
sm_conc = smach.Concurrence(
outcomes=[
'succeeded', 'aborted', 'preempted', 'face_detected'
],
default_outcome='succeeded',
input_keys=['head_left_right', 'head_up_down'],
output_keys=['wave_position', 'wave_yaw_degree'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# sm_conc.StateMachine.add(
# 'move_head',
# move_head_form(head_up_down=head_position),
# transitions={'succeeded': 'Say_Searching',
# 'preempted':'Say_Searching',
# 'aborted':'aborted'})
# sm_conc.StateMachine.add(
# 'Say_Searching',
# text_to_say(text_for_wave_searching, wait = False),
# transitions={'succeeded':'wave_recognition', 'aborted':'wave_recognition', 'preempted':'wave_recognition'})
sm_conc.add('move_head',
move_head_form(head_up_down=head_position))
sm_conc.add('Say_Searching',
text_to_say(text_for_wave_searching, wait=False))
sm_conc.add('wave_recognition', WaveDetection(8.0))
smach.StateMachine.add('Concurrence',
sm_conc,
transitions={
'succeeded': 'Say_Found',
'aborted': 'Move_head_prepare',
'preempted': 'preempted',
'face_detected': 'Say_Found'
})
#Hard-coded maximum time in order to detect wave
# smach.StateMachine.add(
# 'wave_recognition',
# WaveDetection(4.0),
# transitions={'succeeded': 'Say_Found', 'aborted': 'Move_head_prepare',
# 'preempted': 'preempted'})
smach.StateMachine.add(
'Say_Found',
text_to_say("Oh! I have found you finally."),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['delete_database'],
output_keys=['name_face', 'object_name'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
# Say Wave recognize
smach.StateMachine.add(
'say_search_wave',
text_to_say("I'm searching for people waving at me", wait=False),
transitions={'succeeded': 'wave_recognition', 'aborted': 'wave_recognition'})
# Gesture recognition -> Is anyone waving?
smach.StateMachine.add(
'wave_recognition',
WaveDetection(time_for_wave=20),
transitions={'succeeded': 'say_wave_recognize', 'aborted': 'ask_for_person',
'preempted': 'preempted'})
# Say Wave recognize
smach.StateMachine.add(
'say_wave_recognize',
text_to_say("Someone waved to me. I will go there", wait=False),
transitions={'succeeded': 'prepare_coord_wave', 'aborted': 'prepare_coord_wave'})
# Person Recognize
# Prepare the goal to the person that is waving
# TODO: it goes a little far to the person...
smach.StateMachine.add(
'prepare_coord_wave',
prepare_coord_wave(),
transitions={'succeeded': 'go_to_person_wave', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to the person -> we assume that gesture will return the position
smach.StateMachine.add(
'go_to_person_wave',
nav_to_coord('/base_link'),
transitions={'succeeded': 'say_approach_person', 'aborted': 'say_not_approach_person',
'preempted': 'preempted'})
#FAIL Navigation
# Say can't approach person
smach.StateMachine.add(
'say_not_approach_person',
text_to_say("I can't reach where you are. Can anyone come to me, please?"),
transitions={'succeeded': 'wait_for_person', 'aborted': 'wait_for_person'})
# Say approach person
smach.StateMachine.add(
'say_approach_person',
text_to_say("I found you. I am looking at you. I will enroll your face."),
transitions={'succeeded': 'wait_for_person', 'aborted': 'wait_for_person'})
# FAIL Person Recognize
# Ask for person if it can see anyone
smach.StateMachine.add(
'ask_for_person',
text_to_say("I can't see anyone. Can anyone come to me, please?"),
transitions={'succeeded': 'wait_for_person', 'aborted': 'wait_for_person',
'preempted': 'preempted'})
# Wait for person
smach.StateMachine.add(
'wait_for_person',
detect_face(),
transitions={'succeeded': 'say_learn_person', 'aborted': 'ask_for_person'})
# Say approach person
smach.StateMachine.add(
'say_learn_person',
text_to_say("I have detected your face."),
transitions={'succeeded': 'learning_person', 'aborted': 'learning_person'})
# Learn Person -> Ask name + Face Recognition
smach.StateMachine.add(
'learning_person',
SaveFaceSM(time_enroll=5),
transitions={'succeeded': 'ask_order', 'aborted': 'learning_person',
'preempted': 'preempted'})
# Ask for order
smach.StateMachine.add(
'ask_order',
AskQuestionSM("What would you like to order?", GRAMMAR_NAME, True),
transitions={'succeeded': 'process_order', 'aborted': 'ask_order',
'preempted': 'preempted'})
# Process the answer
smach.StateMachine.add(
'process_order',
process_order(),
transitions={'succeeded': 'say_got_it', 'aborted': 'ask_order',
'preempted': 'preempted'})
smach.StateMachine.add(
'calibrate',
CalibrateASR(),
transitions={'succeeded': 'ask_order', 'aborted': 'ask_order',
'preempted': 'preempted'})
# Say what he ask
smach.StateMachine.add(
'say_got_it',
text_to_say(),
transitions={'succeeded': 'succeeded', 'aborted': 'succeeded',
'preempted': 'preempted'})