def __init__(self, minConfidence=90):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['name'],
output_keys=['standard_error', 'face'])
with self:
self.userdata.face = ""
# extract the database that the robot is finding
smach.StateMachine.add('detect_face',
detect_face(minConfidence),
transitions={
'succeeded': 'proces_face',
'aborted': 'aborted',
'preempted': 'preempted'
})
# i filter a little bit
smach.StateMachine.add('proces_face',
proces_face(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
def __init__(self, minConfidence=90):
smach.StateMachine.__init__(
self,
outcomes=["succeeded", "aborted", "preempted"],
input_keys=["name"],
output_keys=["standard_error", "face", "face_frame"],
)
with self:
self.userdata.face = ""
# extract the database that the robot is finding
smach.StateMachine.add(
"detect_face",
detect_face(minConfidence),
transitions={"succeeded": "proces_face", "aborted": "aborted", "preempted": "preempted"},
)
# i filter a little bit
smach.StateMachine.add(
"proces_face",
Process_face(),
transitions={"succeeded": "succeeded", "aborted": "aborted", "preempted": "preempted"},
)
def __init__(self,minConfidence=90):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['name'],
output_keys=['standard_error','face'])
with self:
self.userdata.face = ""
# extract the database that the robot is finding
smach.StateMachine.add(
'detect_face',
detect_face(minConfidence),
transitions={'succeeded': 'proces_face', 'aborted': 'aborted',
'preempted': 'preempted'})
# i filter a little bit
smach.StateMachine.add(
'proces_face',
proces_face(),
transitions={'succeeded': 'succeeded', 'aborted': 'time_sleeper',
'preempted': 'preempted'})
smach.StateMachine.add(
'time_sleeper',
Sleeper(0.5),
transitions={'succeeded': 'detect_face', 'aborted': 'detect_face'})
def __init__(self, minConfidence=90, time_out=0):
smach.StateMachine.__init__(
self,
outcomes=["succeeded", "aborted", "preempted"],
input_keys=["name"],
output_keys=["standard_error", "face", "face_frame"],
)
with self:
self.userdata.face = ""
self.userdata.name = ""
# extract the database that the robot is finding
self.time_init = rospy.Time.now()
smach.StateMachine.add(
"init_time", init_time(), transitions={"succeeded": "detect_face", "aborted": "aborted"}
)
smach.StateMachine.add(
"detect_face",
detect_face(minConfidence, time_topic=1),
transitions={"succeeded": "Process_face", "aborted": "check_time", "preempted": "preempted"},
)
# Change succeeded:proces_face
# i filter a little bit
smach.StateMachine.add(
"Process_face",
Process_face(),
transitions={"succeeded": "Aborting_navigation", "aborted": "time_sleeper", "preempted": "preempted"},
)
smach.StateMachine.add(
"Aborting_navigation",
CancelNavigation(),
transitions={"succeeded": "succeeded", "aborted": "aborted", "preempted": "preempted"},
)
smach.StateMachine.add(
"time_sleeper", Sleeper(0.5), transitions={"succeeded": "check_time", "aborted": "detect_face"}
)
smach.StateMachine.add(
"check_time", check_time(time_out), transitions={"succeeded": "detect_face", "aborted": "aborted"}
)
def __init__(self,minConfidence=90,time_out=0):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['name'],
output_keys=['standard_error','face','face_frame'])
with self:
self.userdata.face = ""
self.userdata.name=""
# extract the database that the robot is finding
self.time_init = rospy.Time.now()
smach.StateMachine.add(
'init_time',
init_time(),
transitions={'succeeded': 'detect_face', 'aborted': 'aborted'})
smach.StateMachine.add(
'detect_face',
detect_face(minConfidence,time_topic=10),
transitions={'succeeded': 'Process_face', 'aborted': 'check_time',
'preempted': 'preempted'})
#Change succeeded:proces_face
# i filter a little bit
smach.StateMachine.add(
'Process_face',
Process_face(),
transitions={'succeeded': 'Aborting_navigation', 'aborted': 'time_sleeper',
'preempted': 'preempted'})
smach.StateMachine.add(
'Aborting_navigation',
CancelNavigation(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'time_sleeper',
Sleeper(0.5),
transitions={'succeeded': 'check_time', 'aborted': 'detect_face'})
smach.StateMachine.add(
'check_time',
check_time(time_out),
transitions={'succeeded': 'detect_face', 'aborted': 'aborted'})
def __init__(self, head_position=None, text_for_wave_searching="I am looking for you."):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
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('Dummy',
DummyStateMachine(),
transitions={'succeeded':'Say_follow'})
smach.StateMachine('Say_follow',
text_to_say('Follow Me Please, I will guide you to the person'),
transitions={'succeeded':'succeeded', 'aborted':'aborted'})
smach.StateMachine.add(
'Move_head_prepare',
prepare_move_head(head_position),
transitions={'succeeded': 'move_head', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'move_head',
move_head_form(head_up_down=head_position),
transitions={'succeeded': 'Say_Searching',
'preempted':'Say_Searching',
'aborted':'aborted'})
smach.StateMachine.add(
'Say_Searching',
text_to_say("text_for_wave_searching"),
transitions={'succeeded':'face_detection', 'aborted':'face_detection', 'preempted':'face_detection'})
#Hard-coded maximum time in order to detect wave
smach.StateMachine.add(
'face_detection',
detect_face(),
transitions={'succeeded': 'Say_Found', 'aborted': 'aborted', #TODO before it was 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"},
)
def __init__(self):
sm = smach.StateMachine.__init__(self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['person_location_coord'])
with self:
self.userdata.name=""
smach.StateMachine.add(
'SetHome',
play_motion_sm('home'),
transitions={'succeeded':'Say_return_Person',
'aborted':'Say_return_Person',
'preempted':'Say_return_Person'})
smach.StateMachine.add(
'Say_return_Person',
text_to_say('I am going to the Entry Door for the Ambulance'),
transitions={'succeeded':'Go_to_Entry_Door', 'aborted':'Go_to_Entry_Door', 'aborted':'Go_to_Entry_Door'})
smach.StateMachine.add(
'Go_to_Entry_Door',
nav_to_poi('entry_door_init'),
transitions={'succeeded':'Say_Ambulance', 'aborted':'Say_Ambulance', 'preempted':'Say_Ambulance'})
#transitions={'succeeded':'Wait_for_Ambulance_Person', 'aborted':'Go_to_Entry_Door', 'preempted':'Go_to_Entry_Door'})
#What is Wait for Ambulance or People Mean? Person detection?
smach.StateMachine.add(
'Wait_for_Ambulance_Person',
Search_Ambulance_Face(),
#Search_Face_Determined('Where are you ambulance?'),
transitions={'succeeded':'Say_Ambulance', 'aborted':'Detect_Fail_Init', 'preempted':'Go_to_Entry_Door'})
smach.StateMachine.add(
'Detect_Fail_Init',
play_motion_sm('home'),
transitions={'succeeded':'Detect_Fail_Execute', 'aborted':'Detect_Fail_Execute', 'preempted':'Detect_Fail_Execute'})
smach.StateMachine.add(
'Detect_Fail_Execute',
detect_face(),
transitions={'succeeded': 'Say_Ambulance', 'aborted': 'Say_Ambulance'})
smach.StateMachine.add(
'Say_Ambulance',
text_to_say("Thank you for arriving as fast as possible. Please Follow Me, I will guide you to the emergency"),
transitions={'succeeded':'Prepare_Emergency_Final', 'aborted':'Prepare_Emergency_Final', 'preempted':'Prepare_Emergency_Final'})
#If Aborted (not supposed to), retry?
smach.StateMachine.add(
'Prepare_Emergency_Final',
prepare_poi_person_emergency(),
transitions={'succeeded':'Go_to_emergency_room_2', 'aborted':'Go_to_emergency_room_2', 'preempted':'Go_to_emergency_room_2'})
smach.StateMachine.add(
'Go_to_emergency_room_2',
nav_to_coord('/map'),
transitions={'succeeded':'Wait_state_emergency', 'aborted':'Go_to_emergency_room_2', 'preempted':'Go_to_emergency_room_2'})
smach.StateMachine.add(
'Wait_state_emergency',
DummyStateMachine(),
transitions={'succeeded':'Say_Finish', 'aborted':'Say_Finish', 'preempted':'Say_Finish'})
smach.StateMachine.add(
'Say_Finish',
text_to_say('Here is the person that is in an emergency situation, please proceed. I helped to save a life.'),
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'],
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('Dummy',
DummyStateMachine(),
transitions={'succeeded': 'Say_follow'})
smach.StateMachine(
'Say_follow',
text_to_say(
'Follow Me Please, I will guide you to the person'),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
smach.StateMachine.add('Move_head_prepare',
prepare_move_head(head_position),
transitions={
'succeeded': 'move_head',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('move_head',
move_head_form(head_up_down=head_position),
transitions={
'succeeded': 'Say_Searching',
'preempted': 'Say_Searching',
'aborted': 'aborted'
})
smach.StateMachine.add('Say_Searching',
text_to_say("text_for_wave_searching"),
transitions={
'succeeded': 'face_detection',
'aborted': 'face_detection',
'preempted': 'face_detection'
})
#Hard-coded maximum time in order to detect wave
smach.StateMachine.add(
'face_detection',
detect_face(),
transitions={
'succeeded': 'Say_Found',
'aborted':
'aborted', #TODO before it was 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, ['succeeded', 'preempted', 'aborted'],
output_keys=['person_location', 'person_location_coord', 'poi_location'])
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
smach.StateMachine.add(
'Home_Play',
play_motion_sm('home'),
transitions={'succeeded':'Search_Person_Room_by_Room','aborted':'Search_Person_Room_by_Room'})
smach.StateMachine.add(
'Search_Person_Room_by_Room',
Search_Emergency_Wave_Room_Change(),
transitions={'succeeded':'Say_Search', 'aborted':'Say_No_People_Found', 'preempted':'Say_Search'})
#This is the worst-case scenario: The person could not be found, so we are losing an important amount of points
smach.StateMachine.add(
'Say_No_People_Found',
text_to_say("I could not find any person in an emergency situation, sorry. Can you come to me?"),
transitions={'succeeded':'face_detection', 'aborted':'aborted'})
#If the person is not found, then it will detect the face
smach.StateMachine.add(
'face_detection',
detect_face(),
transitions={'succeeded': 'Register_Position', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'Say_Search',
text_to_say('Let me help 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':'Go_to_Wave', 'preempted':'Go_to_Wave'})
#The frame_id is '/base_link' because the wave gesture is transformed into this frame, and originally was in xtion
smach.StateMachine.add(
'Go_to_Wave',
nav_to_coord('/base_link'),
transitions={'succeeded':'Say_Arrive_to_Wave', 'aborted':'Go_to_Wave', 'preempted':'Go_to_Wave'})
smach.StateMachine.add(
'Say_Arrive_to_Wave',
text_to_say("I have arrived! "),
transitions={'succeeded':'Register_Position', 'aborted':'Register_Position', 'preempted':'Register_Position'})
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, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['did_pick', 'object_name', 'name_face'],
output_keys=[])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.did_pick = True
smach.StateMachine.add(
'look_to_point',
look_to_point(direction="up",min_duration=1.0),
transitions={'succeeded': 'prepare_say_search_person','preempted':'preempted', 'aborted':'aborted'})
# Say search for person
smach.StateMachine.add(
'prepare_say_search_person',
prepare_searching(),
transitions={'succeeded': 'say_search_person', 'aborted': 'search_for_person',
'preempted': 'preempted'})
# Say search for person
smach.StateMachine.add(
'say_search_person',
text_to_say(),
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',
#go_find_person("party_room"),
recognize_face_concurrent(time_out=15),
transitions={'succeeded': 'say_found_person', 'aborted': 'prepare_ask_for_person_back',
'preempted': 'preempted'})
# We recognize the person
# Say found the person
smach.StateMachine.add(
'say_found_person',
text_to_say("I found you!",wait=False),
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': 'deliver_drink', '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'})
# Person recognition FAILS
# 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': 'wait_for_person_back', 'aborted': 'wait_for_person_back', 'preempted': 'preempted'})
smach.StateMachine.add(
'wait_for_person_back',
detect_face(),
transitions={'succeeded': 'deliver_drink', 'aborted': 'aborted', 'preempted': 'preempted'})
# Deliver part
# Deliver Drink
smach.StateMachine.add(
'deliver_drink',
text_to_say("Ok, I'm going to deliver the drink, please take it"),
transitions={'succeeded': 'check_place_give', 'aborted': 'check_place_give',
'preempted': 'preempted'})
# Check if we pick or ask the drink
smach.StateMachine.add(
'check_place_give',
check_place_give(),
transitions={'succeeded':'pregrasp_state', 'aborted':'Give_Object', 'preempted':'check_place_give'})
# Place if we pick the drink - Pre-grasp position
smach.StateMachine.add(
'pregrasp_state',
play_motion_sm('pre_grasp', skip_planning=True),
transitions={'succeeded': 'succeeded', 'preempted':'pregrasp_state',
'aborted':'pregrasp_state'})
# Give if we ask for drink
smach.StateMachine.add(
'Give_Object',
give_object(),
transitions={'succeeded':'succeeded', 'aborted':'Give_Object', 'preempted':'Give_Object'})
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.nav_to_poi_name = ""
self.userdata.faces = ""
self.userdata.name=''
self.userdata.tts_text = None
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_lang = ''
# Listen the first question
self.userdata.grammar_name = GRAMMAR_NAME
# Find me Part
# Enter room
smach.StateMachine.add(
'say_what_did_you_say',
text_to_say("I'm beginning the what did you say test,. I'm going to the place where ther referee should be"),
#transitions={'succeeded': 'go_location', 'aborted': 'aborted'})
transitions={'succeeded': 'go_location', 'aborted': 'aborted'})
# Go to the location
smach.StateMachine.add(
'go_location',
nav_to_poi("find_me"),
transitions={'succeeded': 'say_faces', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'say_faces',
text_to_say("Searching for faces"),
transitions={'succeeded': 'search_face', 'aborted': 'aborted'})
# Look for a face
smach.StateMachine.add(
'search_face',
SearchFacesSM(),
transitions={'succeeded': 'prepare_coord_person', 'aborted': 'ask_for_tc',
'preempted': 'preempted'})
# Go to the person - We assume that find person will return the position for the person
smach.StateMachine.add(
'prepare_coord_person',
prepare_coord_person(),
transitions={'succeeded': 'go_to_person', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'go_to_person',
nav_to_coord('/base_link'),
transitions={'succeeded': 'say_found', 'aborted': 'aborted',
'preempted': 'preempted'})
# Say "I found you!" + Small Talk
smach.StateMachine.add(
'say_found',
text_to_say("I found you!"),
transitions={'succeeded': 'ActivateASR', 'aborted': 'aborted'})
# Ask for TC if we dont find him
smach.StateMachine.add(
'ask_for_tc',
text_to_say("I can't find you. Can you come to me?"),
transitions={'succeeded': 'wait_for_tc', 'aborted': 'aborted'})
# Wait for TC
smach.StateMachine.add(
'wait_for_tc',
detect_face(),
transitions={'succeeded': 'say_found', 'aborted': 'aborted'})
# Question Part ---------------------------------------------------------------------------------------------
# Init the asr service
# Activate the server
smach.StateMachine.add('ActivateASR',
ActivateASR(),
transitions={'succeeded': 'check_loop', 'aborted': 'aborted', 'preempted': 'preempted'})
# loop test - It checks the number of iterations
smach.StateMachine.add(
'check_loop',
checkLoop(),
transitions={'succeeded': 'ask_next_question', 'aborted': 'aborted',
'preempted': 'preempted', 'end':'DeactivateASR'})
# Ask for next question
smach.StateMachine.add(
'ask_next_question',
text_to_say("I'm ready, ask me a question"),
transitions={'succeeded': 'listen_question', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'listen_question',
ReadASR(),
transitions={'succeeded': 'prepear_repeat', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'prepear_repeat',
prepear_repeat(),
transitions={'succeeded': 'repeat_question', 'aborted': 'aborted',
'preempted': 'preempted'})
# Repeat question
smach.StateMachine.add(
'repeat_question',
text_to_say(),
transitions={'succeeded': 'search_answer', 'aborted': 'aborted',
'preempted': 'preempted'})
# Search the answer
smach.StateMachine.add(
'search_answer',
SelectAnswer(),
transitions={'succeeded': 'say_answer', 'aborted': 'say_answer',
'preempted': 'preempted', 'None': 'aborted'})
# Say the answer
smach.StateMachine.add(
'say_answer',
text_to_say(),
transitions={'succeeded': 'check_loop', 'aborted': 'aborted',
'preempted': 'preempted'})
# Deactivate ASR
smach.StateMachine.add('DeactivateASR',
DeactivateASR(),
transitions={'succeeded': 'say_end', 'aborted': 'aborted', 'preempted': 'preempted'})
smach.StateMachine.add(
'say_end',
text_to_say("What did you say test finished"),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})
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.nav_to_poi_name = ""
self.userdata.faces = ""
self.userdata.name=''
self.userdata.tts_text = None
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_lang = ''
# Listen the first question
self.userdata.grammar_name = GRAMMAR_NAME
# Home position
smach.StateMachine.add(
'home_position_init',
play_motion_sm('home'),
transitions={'succeeded': 'say_what_did_you_say', 'aborted': 'home_position_init', #TODO: Change aborted to try again
'preempted': 'preempted'})
# Enter room
smach.StateMachine.add(
'say_what_did_you_say',
text_to_say("I'm beginning the what did you say test,. I'm going to the place where the referee should be"),
#transitions={'succeeded': 'go_location', 'aborted': 'aborted'})
transitions={'succeeded': 'go_location', 'aborted': 'aborted'})
# Go to the location
smach.StateMachine.add(
'go_location',
nav_to_poi("init_what_say"),
transitions={'succeeded': 'search_face', 'aborted': 'aborted',
'preempted': 'preempted'})
# smach.StateMachine.add(
# 'say_faces',
# text_to_say("Searching for faces"),
# transitions={'succeeded': 'search_face', 'aborted': 'aborted'})
# Look for a face
smach.StateMachine.add(
'search_face',
go_find_person("init_what_say"),
transitions={'succeeded': 'Say_Found_Face', 'aborted': 'ask_for_tc',
'preempted': 'preempted'},
remapping={'face_frame':'face_frame'})
# Go to the person - We assume that find person will return the position for the person
smach.StateMachine.add(
'Say_Found_Face',
text_to_say('Referee! I have found you at last. Now I am going to you, wait for me.'),
transitions={'succeeded': 'get_current_pose_yaw', 'aborted': 'Say_Found_Face',
'preempted': 'preempted'})
smach.StateMachine.add(
'get_current_pose_yaw',
get_current_robot_pose(),
transitions={'succeeded': 'prepare_coord_person', 'aborted': 'ask_for_tc',
'preempted': 'preempted'})
smach.StateMachine.add(
'prepare_coord_person',
prepare_coord_person(),
transitions={'succeeded': 'go_to_person', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'go_to_person',
nav_to_coord_ud(),
transitions={'succeeded': 'say_found', 'aborted': 'Say_person_not_reached',
'preempted': 'preempted'})
smach.StateMachine.add(
'Say_person_not_reached',
text_to_say('I Found you, but cannot reach you, can you come to me please?'),
transitions={'succeeded': 'ActivateASR', 'aborted': 'aborted'})
# Say "I found you!" + Small Talk
smach.StateMachine.add(
'say_found',
text_to_say("I found you! I have arrived to where you are."),
transitions={'succeeded': 'ActivateASR', 'aborted': 'aborted'})
# Ask for TC if we don't find him
smach.StateMachine.add(
'ask_for_tc',
text_to_say("I can't find you. Can you come to me?"),
transitions={'succeeded': 'wait_for_tc', 'aborted': 'aborted'})
# Wait for TC
smach.StateMachine.add(
'wait_for_tc',
detect_face(),
transitions={'succeeded': 'say_found', 'aborted': 'aborted'})
# Question Part ---------------------------------------------------------------------------------------------
# Init the asr service
# Activate the server
smach.StateMachine.add('ActivateASR',
ActivateASR(),
transitions={'succeeded': 'check_loop', 'aborted': 'aborted', 'preempted': 'preempted'})
# loop test - It checks the number of iterations
smach.StateMachine.add(
'check_loop',
checkLoop(),
transitions={'succeeded': 'ask_next_question', 'aborted': 'aborted',
'preempted': 'preempted', 'end':'DeactivateASR'})
# Ask for next question
smach.StateMachine.add(
'ask_next_question',
text_to_say("I'm ready, ask me a question"),
transitions={'succeeded': 'listen_question', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'listen_question',
ReadASR(),
transitions={'succeeded': 'prepear_repeat', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'prepear_repeat',
prepear_repeat(),
transitions={'succeeded': 'repeat_question', 'aborted': 'aborted',
'preempted': 'preempted'})
# Repeat question
smach.StateMachine.add(
'repeat_question',
text_to_say(),
transitions={'succeeded': 'search_answer', 'aborted': 'aborted',
'preempted': 'preempted'})
# Search the answer
smach.StateMachine.add(
'search_answer',
SelectAnswer(),
transitions={'succeeded': 'say_answer', 'aborted': 'say_answer',
'preempted': 'preempted', 'None': 'aborted'})
# Say the answer
smach.StateMachine.add(
'say_answer',
text_to_say(),
transitions={'succeeded': 'check_loop', 'aborted': 'aborted',
'preempted': 'preempted'})
# Deactivate ASR
smach.StateMachine.add('DeactivateASR',
DeactivateASR(),
transitions={'succeeded': 'say_end', 'aborted': 'aborted', 'preempted': 'preempted'})
smach.StateMachine.add(
'say_end',
text_to_say("What did you say test finished"),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})
def __init__(self):
sm = smach.StateMachine.__init__(
self, outcomes=["succeeded", "preempted", "aborted"], input_keys=["person_location_coord"]
)
with self:
self.userdata.name = ""
smach.StateMachine.add(
"SetHome",
play_motion_sm("home"),
transitions={
"succeeded": "Say_return_Person",
"aborted": "Say_return_Person",
"preempted": "Say_return_Person",
},
)
smach.StateMachine.add(
"Say_return_Person",
text_to_say("I am going to the Entry Door for the Ambulance"),
transitions={
"succeeded": "Go_to_Entry_Door",
"aborted": "Go_to_Entry_Door",
"aborted": "Go_to_Entry_Door",
},
)
smach.StateMachine.add(
"Go_to_Entry_Door",
nav_to_poi("entry_door_init"),
transitions={
"succeeded": "Wait_for_Ambulance_Person",
"aborted": "Go_to_Entry_Door",
"preempted": "Go_to_Entry_Door",
},
)
# What is Wait for Ambulance or People Mean? Person detection?
smach.StateMachine.add(
"Wait_for_Ambulance_Person",
Search_Ambulance_Face(),
# Search_Face_Determined('Where are you ambulance?'),
transitions={
"succeeded": "Say_Ambulance",
"aborted": "Detect_Fail_Init",
"preempted": "Go_to_Entry_Door",
},
)
smach.StateMachine.add(
"Detect_Fail_Init",
play_motion_sm("home"),
transitions={
"succeeded": "Detect_Fail_Execute",
"aborted": "Detect_Fail_Execute",
"preempted": "Detect_Fail_Execute",
},
)
smach.StateMachine.add(
"Detect_Fail_Execute",
detect_face(),
transitions={"succeeded": "Say_Ambulance", "aborted": "Say_Ambulance"},
)
smach.StateMachine.add(
"Say_Ambulance",
text_to_say(
"Thank you for arriving as fast as possible. Please Follow Me, I will guide you to the emergency"
),
transitions={
"succeeded": "Prepare_Emergency_Final",
"aborted": "Prepare_Emergency_Final",
"preempted": "Prepare_Emergency_Final",
},
)
# If Aborted (not supposed to), retry?
smach.StateMachine.add(
"Prepare_Emergency_Final",
prepare_poi_person_emergency(),
transitions={
"succeeded": "Go_to_emergency_room_2",
"aborted": "Go_to_emergency_room_2",
"preempted": "Go_to_emergency_room_2",
},
)
smach.StateMachine.add(
"Go_to_emergency_room_2",
nav_to_coord("/map"),
transitions={
"succeeded": "Wait_state_emergency",
"aborted": "Go_to_emergency_room_2",
"preempted": "Go_to_emergency_room_2",
},
)
smach.StateMachine.add(
"Wait_state_emergency",
DummyStateMachine(),
transitions={"succeeded": "Say_Finish", "aborted": "Say_Finish", "preempted": "Say_Finish"},
)
smach.StateMachine.add(
"Say_Finish",
text_to_say(
"Here is the person that is in an emergency situation, please proceed. I helped to save a life."
),
transitions={"succeeded": "succeeded", "aborted": "aborted", "preempted": "preempted"},
)
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.try_iterations = 1
self.userdata.gesture_name = ''
self.userdata.object_name = ""
self.userdata.manip_time_to_play = 4
self.userdata.did_pick = True
self.userdata.grammar_name = GRAMMAR_NAME
smach.StateMachine.add(
'init_cocktail',
text_to_say("Ready for cocktail party"),
transitions={'succeeded': 'learning_person', '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("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(),
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'})
# 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': 'ask_for_person'})
# 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 order?", GRAMMAR_NAME),
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'})
# Say what he ask
smach.StateMachine.add(
'say_got_it',
text_to_say("I got it!"),
transitions={'succeeded': 'go_to_storage', 'aborted': 'go_to_storage',
'preempted': 'preempted'})
# Go to the storage_room
smach.StateMachine.add(
'go_to_storage',
nav_to_poi("storage_room"),
transitions={'succeeded': 'recognize_object_and_pick', 'aborted': 'go_to_storage',
'preempted': 'preempted'})
# Recognize and pick object if found
smach.StateMachine.add(
'recognize_object_and_pick',
RecObjectAndPick(),
transitions={'succeeded': 'go_to_party',
'fail_grasp':'go_to_party',
'fail_recognize': 'try_again_recognition'})
# Say start object recognition
# smach.StateMachine.add(
# 'say_start_obj_recognition',
# text_to_say("I'm going to start the Object recognition"),
# transitions={'succeeded': 'object_recognition', 'aborted': 'object_recognition'})
#
# # Do object_recognition
# smach.StateMachine.add(
# 'object_recognition',
# dummy_recognize(),
# transitions={'succeeded': 'say_grasp_object', 'aborted': 'say_release_obj',
# 'preempted': 'preempted'})
#
# # Say grasp object
# smach.StateMachine.add(
# 'say_grasp_order',
# text_to_say("I'm going to grasp it"),
# transitions={'succeeded': 'grasp_food_order', 'aborted': 'Grasp_fail_Ask_Person',
# 'preempted': 'preempted'})
#
# # Grasp Object
# smach.StateMachine.add(
# 'grasp_food_order',
# pick_object_sm(),
# transitions={'succeeded': 'go_to_party', 'aborted': 'Grasp_fail_Ask_Person',
# 'preempted': 'preempted'})
# We don't recognized the object
smach.StateMachine.add(
'try_again_recognition',
checkRecognition(),
transitions={'succeeded': 'recognize_object_and_pick', 'aborted': 'recognize_object_and_pick',
'preempted': 'preempted', 'end':'say_fail_recognize'})
# Say fail recognize objects
smach.StateMachine.add(
'say_fail_recognize',
text_to_say("I'm not able to recognized any object."),
transitions={'succeeded': 'Grasp_fail_Ask_Person', 'aborted': 'Grasp_fail_Ask_Person'})
# 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':'change_did_pick', 'aborted':'change_did_pick', 'preempted':'change_did_pick'})
smach.StateMachine.add(
'change_did_pick',
change_did_pick(),
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'})
# Check if we pick or ask the drink
smach.StateMachine.add(
'check_place_give',
check_place_give(),
transitions={'succeeded':'Place_Object', 'aborted':'Give_Object', 'preempted':'check_place_give'})
# Place if we pick the drink
smach.StateMachine.add(
'Place_Object',
place_object_sm(),
transitions={'succeeded':'check_loop', 'aborted':'Place_Object', 'preempted':'Place_Object'})
# Give if we ask for drink
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'})
# Say leaving the arena
smach.StateMachine.add(
'say_leaving_arena',
text_to_say("I finished the cocktail party, I'm leaving the arena"),
transitions={'succeeded': 'Give_Object', 'aborted': 'Give_Object',
'preempted': 'preempted'})
# Leaving the arena
smach.StateMachine.add(
'leaving_arena',
nav_to_poi('leave_arena'),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['did_pick', 'object_name', 'name_face'],
output_keys=[])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.did_pick = True
smach.StateMachine.add('look_to_point',
look_to_point(direction="up",
min_duration=1.0),
transitions={
'succeeded':
'prepare_say_search_person',
'preempted': 'preempted',
'aborted': 'aborted'
})
# Say search for person
smach.StateMachine.add('prepare_say_search_person',
prepare_searching(),
transitions={
'succeeded': 'say_search_person',
'aborted': 'search_for_person',
'preempted': 'preempted'
})
# Say search for person
smach.StateMachine.add('say_search_person',
text_to_say(),
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',
#go_find_person("party_room"),
recognize_face_concurrent(time_out=15),
transitions={
'succeeded': 'say_found_person',
'aborted': 'prepare_ask_for_person_back',
'preempted': 'preempted'
})
# We recognize the person
# Say found the person
smach.StateMachine.add('say_found_person',
text_to_say("I found you!", wait=False),
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': 'deliver_drink',
'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'
})
# Person recognition FAILS
# 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': 'wait_for_person_back',
'aborted': 'wait_for_person_back',
'preempted': 'preempted'
})
smach.StateMachine.add('wait_for_person_back',
detect_face(),
transitions={
'succeeded': 'deliver_drink',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Deliver part
# Deliver Drink
smach.StateMachine.add(
'deliver_drink',
text_to_say(
"Ok, I'm going to deliver the drink, please take it"),
transitions={
'succeeded': 'check_place_give',
'aborted': 'check_place_give',
'preempted': 'preempted'
})
# Check if we pick or ask the drink
smach.StateMachine.add('check_place_give',
check_place_give(),
transitions={
'succeeded': 'pregrasp_state',
'aborted': 'Give_Object',
'preempted': 'check_place_give'
})
# Place if we pick the drink - Pre-grasp position
smach.StateMachine.add('pregrasp_state',
play_motion_sm('pre_grasp',
skip_planning=True),
transitions={
'succeeded': 'succeeded',
'preempted': 'pregrasp_state',
'aborted': 'pregrasp_state'
})
# Give if we ask for drink
smach.StateMachine.add('Give_Object',
give_object(),
transitions={
'succeeded': 'succeeded',
'aborted': 'Give_Object',
'preempted': 'Give_Object'
})
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.nav_to_poi_name = ""
self.userdata.faces = ""
self.userdata.name = ''
self.userdata.tts_text = None
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_lang = ''
# Listen the first question
self.userdata.grammar_name = GRAMMAR_NAME
# Find me Part
# Enter room
smach.StateMachine.add(
'say_what_did_you_say',
text_to_say(
"I'm beginning the what did you say test,. I'm going to the place where ther referee should be"
),
#transitions={'succeeded': 'go_location', 'aborted': 'aborted'})
transitions={
'succeeded': 'go_location',
'aborted': 'aborted'
})
# Go to the location
smach.StateMachine.add('go_location',
nav_to_poi("find_me"),
transitions={
'succeeded': 'say_faces',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('say_faces',
text_to_say("Searching for faces"),
transitions={
'succeeded': 'search_face',
'aborted': 'aborted'
})
# Look for a face
smach.StateMachine.add('search_face',
SearchFacesSM(),
transitions={
'succeeded': 'prepare_coord_person',
'aborted': 'ask_for_tc',
'preempted': 'preempted'
})
# Go to the person - We assume that find person will return the position for the person
smach.StateMachine.add('prepare_coord_person',
prepare_coord_person(),
transitions={
'succeeded': 'go_to_person',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('go_to_person',
nav_to_coord('/base_link'),
transitions={
'succeeded': 'say_found',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Say "I found you!" + Small Talk
smach.StateMachine.add('say_found',
text_to_say("I found you!"),
transitions={
'succeeded': 'ActivateASR',
'aborted': 'aborted'
})
# Ask for TC if we dont find him
smach.StateMachine.add(
'ask_for_tc',
text_to_say("I can't find you. Can you come to me?"),
transitions={
'succeeded': 'wait_for_tc',
'aborted': 'aborted'
})
# Wait for TC
smach.StateMachine.add('wait_for_tc',
detect_face(),
transitions={
'succeeded': 'say_found',
'aborted': 'aborted'
})
# Question Part ---------------------------------------------------------------------------------------------
# Init the asr service
# Activate the server
smach.StateMachine.add('ActivateASR',
ActivateASR(),
transitions={
'succeeded': 'check_loop',
'aborted': 'aborted',
'preempted': 'preempted'
})
# loop test - It checks the number of iterations
smach.StateMachine.add('check_loop',
checkLoop(),
transitions={
'succeeded': 'ask_next_question',
'aborted': 'aborted',
'preempted': 'preempted',
'end': 'DeactivateASR'
})
# Ask for next question
smach.StateMachine.add('ask_next_question',
text_to_say("I'm ready, ask me a question"),
transitions={
'succeeded': 'listen_question',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('listen_question',
ReadASR(),
transitions={
'succeeded': 'prepear_repeat',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('prepear_repeat',
prepear_repeat(),
transitions={
'succeeded': 'repeat_question',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Repeat question
smach.StateMachine.add('repeat_question',
text_to_say(),
transitions={
'succeeded': 'search_answer',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Search the answer
smach.StateMachine.add('search_answer',
SelectAnswer(),
transitions={
'succeeded': 'say_answer',
'aborted': 'say_answer',
'preempted': 'preempted',
'None': 'aborted'
})
# Say the answer
smach.StateMachine.add('say_answer',
text_to_say(),
transitions={
'succeeded': 'check_loop',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Deactivate ASR
smach.StateMachine.add('DeactivateASR',
DeactivateASR(),
transitions={
'succeeded': 'say_end',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add(
'say_end',
text_to_say("What did you say test finished"),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
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.nav_to_poi_name = ""
self.userdata.faces = ""
self.userdata.name=''
self.userdata.tts_text = None
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_lang = ''
# Listen the first question
self.userdata.grammar_name = GRAMMAR_NAME
# Home position
smach.StateMachine.add(
'home_position_init',
play_motion_sm('home'),
transitions={'succeeded': 'ActivateASR', 'aborted': 'home_position_init', #TODO: Change aborted to try again
'preempted': 'preempted'})
# Enter room
smach.StateMachine.add(
'say_what_did_you_say',
text_to_say("I'm beginning the what did you say test, I'm going to looking for the referee"),
#transitions={'succeeded': 'go_location', 'aborted': 'aborted'})
transitions={'succeeded': 'search_face', 'aborted': 'search_face'})
# # Go to the location
# smach.StateMachine.add(
# 'go_location',
# nav_to_poi("init_what_say"),
# transitions={'succeeded': 'search_face', 'aborted': 'aborted',
# 'preempted': 'preempted'})
# smach.StateMachine.add(
# 'say_faces',
# text_to_say("Searching for faces"),
# transitions={'succeeded': 'search_face', 'aborted': 'aborted'})
# Look for a face
smach.StateMachine.add(
'search_face',
go_find_person(),
transitions={'succeeded': 'Say_Found_Face', 'aborted': 'ask_for_tc',
'preempted': 'preempted'},
remapping={'face_frame':'face_frame'})
# Go to the person - We assume that find person will return the position for the person
smach.StateMachine.add(
'Say_Found_Face',
text_to_say('Referee! I have found you at last. Now I am going to you, wait for me.'),
transitions={'succeeded': 'get_current_pose_yaw', 'aborted': 'Say_Found_Face',
'preempted': 'preempted'})
smach.StateMachine.add(
'get_current_pose_yaw',
get_current_robot_pose(),
transitions={'succeeded': 'prepare_coord_person', 'aborted': 'ask_for_tc',
'preempted': 'preempted'})
smach.StateMachine.add(
'prepare_coord_person',
prepare_coord_person(),
transitions={'succeeded': 'go_to_person', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'go_to_person',
nav_to_coord_ud(),
transitions={'succeeded': 'say_found', 'aborted': 'Say_person_not_reached',
'preempted': 'preempted'})
smach.StateMachine.add(
'Say_person_not_reached',
text_to_say('I Found you, but cannot reach you, can you come to me please?'),
transitions={'succeeded': 'wait_for_tc', 'aborted': 'aborted'})
# Say "I found you!" + Small Talk
smach.StateMachine.add(
'say_found',
text_to_say("I found you! I have arrived to where you are."),
transitions={'succeeded': 'ActivateASR', 'aborted': 'aborted'})
# Ask for TC if we don't find him
smach.StateMachine.add(
'ask_for_tc',
text_to_say("I can't find you. Can you come to me?"),
transitions={'succeeded': 'wait_for_tc', 'aborted': 'aborted'})
# Wait for TC
smach.StateMachine.add(
'wait_for_tc',
detect_face(),
transitions={'succeeded': 'say_found', 'aborted': 'aborted'})
# Question Part ---------------------------------------------------------------------------------------------
# Init the asr service
# Activate the server
smach.StateMachine.add('ActivateASR',
ActivateASR(),
transitions={'succeeded': 'check_loop', 'aborted': 'aborted', 'preempted': 'preempted'})
# loop test - It checks the number of iterations
smach.StateMachine.add(
'check_loop',
checkLoop(),
transitions={'succeeded': 'ask_next_question', 'aborted': 'aborted',
'preempted': 'preempted', 'end':'DeactivateASR'})
# Ask for next question
smach.StateMachine.add(
'ask_next_question',
text_to_say("I'm ready, ask me a question"),
transitions={'succeeded': 'listen_question', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'listen_question',
ReadASR(),
transitions={'succeeded': 'prepear_repeat', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'prepear_repeat',
prepear_repeat(),
transitions={'succeeded': 'repeat_question', 'aborted': 'aborted',
'preempted': 'preempted'})
# Repeat question
smach.StateMachine.add(
'repeat_question',
text_to_say(),
transitions={'succeeded': 'search_answer', 'aborted': 'aborted',
'preempted': 'preempted'})
# Search the answer
smach.StateMachine.add(
'search_answer',
SelectAnswer(),
transitions={'succeeded': 'say_answer', 'aborted': 'say_answer',
'preempted': 'preempted', 'None': 'aborted'})
# Say the answer
smach.StateMachine.add(
'say_answer',
text_to_say(),
transitions={'succeeded': 'check_loop', 'aborted': 'aborted',
'preempted': 'preempted'})
# Deactivate ASR
smach.StateMachine.add('DeactivateASR',
DeactivateASR(),
transitions={'succeeded': 'say_end', 'aborted': 'aborted', 'preempted': 'preempted'})
smach.StateMachine.add(
'say_end',
text_to_say("What did you say test finished"),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
output_keys=[
'person_location',
'person_location_coord', 'poi_location'
])
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
smach.StateMachine.add('Home_Play',
play_motion_sm('home'),
transitions={
'succeeded':
'Search_Person_Room_by_Room',
'aborted': 'Search_Person_Room_by_Room'
})
smach.StateMachine.add('Search_Person_Room_by_Room',
Search_Emergency_Wave_Room_Change(),
transitions={
'succeeded': 'Say_Search',
'aborted': 'Say_No_People_Found',
'preempted': 'Say_Search'
})
#This is the worst-case scenario: The person could not be found, so we are losing an important amount of points
smach.StateMachine.add(
'Say_No_People_Found',
text_to_say(
"I could not find any person in an emergency situation, sorry. Can you come to me?"
),
transitions={
'succeeded': 'face_detection',
'aborted': 'aborted'
})
#If the person is not found, then it will detect the face
smach.StateMachine.add('face_detection',
detect_face(),
transitions={
'succeeded': 'Register_Position',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('Say_Search',
text_to_say('Let me help 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': 'Go_to_Wave',
'preempted': 'Go_to_Wave'
})
#The frame_id is '/base_link' because the wave gesture is transformed into this frame, and originally was in xtion
smach.StateMachine.add('Go_to_Wave',
nav_to_coord('/base_link'),
transitions={
'succeeded': 'Say_Arrive_to_Wave',
'aborted': 'Go_to_Wave',
'preempted': 'Go_to_Wave'
})
smach.StateMachine.add('Say_Arrive_to_Wave',
text_to_say("I have arrived! "),
transitions={
'succeeded': 'Register_Position',
'aborted': 'Register_Position',
'preempted': 'Register_Position'
})
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, 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': 'learning_person', 'aborted': 'go_to_person_wave',
'preempted': 'preempted'})
# 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': 'ask_for_person',
'preempted': 'preempted'})
# Wait for person
smach.StateMachine.add(
'wait_for_person',
detect_face(),
transitions={'succeeded': 'learning_person', 'aborted': 'ask_for_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),
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'})
# Say what he ask
smach.StateMachine.add(
'say_got_it',
text_to_say("I got it!"),
transitions={'succeeded': 'succeeded', 'aborted': 'succeeded',
'preempted': 'preempted'})
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, minConfidence=90, time_out=0):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['name'],
output_keys=['standard_error', 'face', 'face_frame'])
with self:
self.userdata.face = ""
self.userdata.name = ""
# extract the database that the robot is finding
self.time_init = rospy.Time.now()
smach.StateMachine.add('init_time',
init_time(),
transitions={
'succeeded': 'detect_face',
'aborted': 'aborted'
})
smach.StateMachine.add('detect_face',
detect_face(minConfidence, time_topic=10),
transitions={
'succeeded': 'Process_face',
'aborted': 'check_time',
'preempted': 'preempted'
})
#Change succeeded:proces_face
# i filter a little bit
smach.StateMachine.add('Process_face',
Process_face(),
transitions={
'succeeded': 'Aborting_navigation',
'aborted': 'time_sleeper',
'preempted': 'preempted'
})
smach.StateMachine.add('Aborting_navigation',
CancelNavigation(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('time_sleeper',
Sleeper(0.5),
transitions={
'succeeded': 'check_time',
'aborted': 'detect_face'
})
smach.StateMachine.add('check_time',
check_time(time_out),
transitions={
'succeeded': 'detect_face',
'aborted': 'aborted'
})
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'})
