def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['name', 'nav_to_poi_name', 'face'],
output_keys=['face', 'standard_error'])
with self:
self.userdata.num_iterations = 0
self.userdata.face = None
self.userdata.wait_time = 15
# We define the different points
smach.StateMachine.add('prepare_poi',
prepare_poi(),
transitions={
'succeeded': 'Concurrence',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Concurrence
sm_conc = smach.Concurrence(
outcomes=['succeeded', 'aborted', 'preempted', 'endTime'],
default_outcome='succeeded',
input_keys=['name', 'nav_to_poi_name', 'face', 'wait_time'],
output_keys=['face', 'standard_error'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# Go around the room
smach.Concurrence.add('walk_to_poi', nav_to_poi())
# Move head
smach.Concurrence.add('TimeOut', TimeOut())
# Search for face
smach.Concurrence.add('find_faces',
recognize_face_concurrent())
smach.StateMachine.add('Concurrence',
sm_conc,
transitions={
'succeeded': 'succeeded',
'aborted': 'prepare_poi',
'endTime': 'aborted'
})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['name', 'nav_to_poi_name', 'face', 'face_frame'],
output_keys=['face', 'standard_error', 'face_frame'])
with self:
self.userdata.num_iterations = 0
self.userdata.face = None
self.userdata.wait_time = 5
smach.StateMachine.add(
'Say_Searching',
text_to_say('Right Now I am looking for the Ambulance.'),
transitions={
'succeeded': 'Concurrence',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Concurrence
sm_conc = smach.Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
input_keys=['name', 'nav_to_poi_name', 'face', 'wait_time'],
output_keys=['face', 'standard_error', 'face_frame'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# Search for face
smach.Concurrence.add('find_faces',
recognize_face_concurrent())
smach.Concurrence.add('say_search_faces',
say_searching_faces())
smach.StateMachine.add('Concurrence',
sm_conc,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'aborted'
})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['name', 'nav_to_poi_name', 'face'],
output_keys=['face', 'standard_error'])
with self:
self.userdata.num_iterations = 0
self.userdata.face = None
self.userdata.wait_time = 15
# We define the different points
smach.StateMachine.add(
'prepare_poi',
prepare_poi(),
transitions={'succeeded': 'Concurrence', 'aborted': 'aborted',
'preempted': 'preempted'})
# Concurrence
sm_conc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted', 'endTime'],
default_outcome='succeeded',
input_keys=['name', 'nav_to_poi_name', 'face', 'wait_time'],
output_keys=['face', 'standard_error'],
child_termination_cb = child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# Go around the room
smach.Concurrence.add('walk_to_poi', nav_to_poi())
# Move head
smach.Concurrence.add('TimeOut', TimeOut())
# Search for face
smach.Concurrence.add('find_faces', recognize_face_concurrent())
smach.StateMachine.add('Concurrence', sm_conc,
transitions={'succeeded':'succeeded', 'aborted':'prepare_poi', 'endTime': 'aborted'})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['name', 'nav_to_poi_name', 'face','face_frame'],
output_keys=['face', 'standard_error', 'face_frame'])
with self:
self.userdata.num_iterations = 0
self.userdata.face = None
self.userdata.wait_time = 5
smach.StateMachine.add(
'Say_Searching',
text_to_say('Right Now I am looking for the Ambulance.'),
transitions={'succeeded': 'Concurrence', 'aborted': 'aborted',
'preempted': 'preempted'})
# Concurrence
sm_conc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
input_keys=['name', 'nav_to_poi_name', 'face', 'wait_time'],
output_keys=['face', 'standard_error', 'face_frame'],
child_termination_cb = child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# Search for face
smach.Concurrence.add('find_faces', recognize_face_concurrent())
smach.Concurrence.add('say_search_faces', say_searching_faces())
smach.StateMachine.add('Concurrence', sm_conc,
transitions={'succeeded':'succeeded',
'aborted':'aborted',
'preempted':'aborted'})
def __init__(self,poi_name=None,time_out=10):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['name', 'nav_to_poi_name', 'face','face_frame'],
output_keys=['face', 'standard_error', 'face_frame'])
self.poi_name=poi_name
self.time_out=time_out
with self:
self.userdata.num_iterations = 0
self.userdata.face = None
self.userdata.wait_time = 5
self.userdata.nav_to_poi_name=None
self.userdata.face_frame=None
# We define the different points
smach.StateMachine.add(
'prepare_poi',
prepare_poi(self.poi_name),
transitions={'poi': 'go_poi', 'no_poi': 'Concurrence'})
smach.StateMachine.add(
'Say_Searching',
text_to_say('Right Now I am looking for you.',wait=True),
transitions={'succeeded': 'go_poi', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'go_poi',
nav_to_poi(poi_name),
transitions={'succeeded': 'Concurrence', 'aborted': 'Concurrence', #TODO
'preempted': 'preempted'})
# Concurrence
sm_conc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted', 'endTime'],
default_outcome='succeeded',
input_keys=['name', 'nav_to_poi_name', 'face', 'wait_time'],
output_keys=['face', 'standard_error', 'face_frame'],
child_termination_cb = child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# Go around the room
# smach.Concurrence.add('walk_to_poi', nav_to_poi())
#smach.Concurrence.add('turn', turn_infinit('left'))
# Move head
smach.Concurrence.add('TimeOut', TimeOut(self.time_out))
# Search for face
smach.Concurrence.add('find_faces', recognize_face_concurrent())
smach.Concurrence.add('say_search_faces', say_searching_faces())
smach.StateMachine.add('Concurrence', sm_conc,
transitions={'succeeded':'DEFAULT_POSITION',
'aborted':'Say_Finish',
'endTime': 'Say_Finish',
'preempted':'Say_Finish'})
smach.StateMachine.add(
'Say_Finish',
text_to_say('I have finish with no exit the search',wait=False),
transitions={'succeeded': 'DEFAULT_POSITION_NO', 'aborted': 'DEFAULT_POSITION_NO',
'preempted': 'preempted'})
smach.StateMachine.add('DEFAULT_POSITION_NO',
move_head_form("center","up"),
transitions={'succeeded': 'aborted','aborted':'aborted'})
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center","up"),
transitions={'succeeded': 'succeeded','aborted':'succeeded'})
def __init__(self,person_name):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['face','face_frame'],
output_keys=['face', 'standard_error', 'face_frame'])
with self:
self.userdata.name = person_name
self.userdata.num_iterations = 0
self.userdata.face = None
self.userdata.wait_time = 50
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_text = ''
self.userdata.tts_lang = 'en_US'
smach.StateMachine.add(
'Name_Preparation',
NamePreparation(person_name),
transitions={'succeeded': 'Say_Searching', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'Say_Searching',
text_to_say('Right Now I am looking for you.'),
transitions={'succeeded': 'Concurrence', 'aborted': 'aborted',
'preempted': 'preempted'})
# Concurrence
sm_conc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted', 'endTime'],
default_outcome='succeeded',
input_keys=['name', 'face', 'wait_time'],
output_keys=['face', 'standard_error', 'face_frame'],
child_termination_cb = child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# Go around the room
smach.Concurrence.add('turn', turn(120))
# Move head
#smach.Concurrence.add('TimeOut', TimeOut(1000))
# Search for face
smach.Concurrence.add('find_faces', recognize_face_concurrent())
smach.Concurrence.add('say_search_faces', say_searching_faces())
smach.StateMachine.add('Concurrence', sm_conc,
transitions={'succeeded':'succeeded',
'aborted':'aborted',
'endTime': 'aborted',
'preempted':'Say_Changing_Poi'})
smach.StateMachine.add('Say_Changing_Poi',
text_to_say('I am going to change my position so I can search for faces'),
transitions={'succeeded': 'Say_Searching', 'aborted': 'aborted',
'preempted': 'preempted'})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['name', 'nav_to_poi_name', 'face', 'face_frame'],
output_keys=['face', 'standard_error', 'face_frame'])
with self:
self.userdata.num_iterations = 0
self.userdata.face = None
self.userdata.wait_time = 5
# We define the different points
smach.StateMachine.add('prepare_poi',
prepare_poi(),
transitions={
'succeeded': 'Say_Searching',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add(
'Say_Searching',
text_to_say('Right Now I am looking for you.'),
transitions={
'succeeded': 'Concurrence',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Concurrence
sm_conc = smach.Concurrence(
outcomes=['succeeded', 'aborted', 'preempted', 'endTime'],
default_outcome='succeeded',
input_keys=['name', 'nav_to_poi_name', 'face', 'wait_time'],
output_keys=['face', 'standard_error', 'face_frame'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# Go around the room
smach.Concurrence.add('walk_to_poi', nav_to_poi())
# Move head
smach.Concurrence.add('TimeOut', TimeOut())
# Search for face
smach.Concurrence.add('find_faces',
recognize_face_concurrent())
smach.Concurrence.add('say_search_faces',
say_searching_faces())
smach.StateMachine.add('Concurrence',
sm_conc,
transitions={
'succeeded': 'succeeded',
'aborted': 'Say_Changing_Poi',
'endTime': 'Say_Changing_Poi',
'preempted': 'Say_Changing_Poi'
})
smach.StateMachine.add(
'Say_Changing_Poi',
text_to_say(
'I am going to change my position so I can search for faces'
),
transitions={
'succeeded': 'prepare_poi',
'aborted': 'aborted',
'preempted': 'preempted'
})
def __init__(self, poi_name=None, time_out=10):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['name', 'nav_to_poi_name', 'face', 'face_frame'],
output_keys=['face', 'standard_error', 'face_frame'])
self.poi_name = poi_name
self.time_out = time_out
with self:
self.userdata.name = ''
self.userdata.num_iterations = 0
self.userdata.face = None
self.userdata.wait_time = 5
self.userdata.nav_to_poi_name = None
self.userdata.face_frame = None
# We define the different points
smach.StateMachine.add('prepare_poi',
prepare_poi(self.poi_name),
transitions={
'poi': 'go_poi',
'no_poi': 'Concurrence'
})
smach.StateMachine.add('Say_Searching',
text_to_say(
'Right Now I am looking for you.',
wait=True),
transitions={
'succeeded': 'go_poi',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add(
'go_poi',
nav_to_poi(poi_name),
transitions={
'succeeded': 'Concurrence',
'aborted': 'Concurrence', #TODO
'preempted': 'preempted'
})
# Concurrence
sm_conc = smach.Concurrence(
outcomes=['succeeded', 'aborted', 'preempted', 'endTime'],
default_outcome='succeeded',
input_keys=['name', 'nav_to_poi_name', 'face', 'wait_time'],
output_keys=['face', 'standard_error', 'face_frame'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# Go around the room
# smach.Concurrence.add('walk_to_poi', nav_to_poi())
#smach.Concurrence.add('turn', turn_infinit('left'))
# Move head
smach.Concurrence.add('TimeOut', TimeOut(self.time_out))
# Search for face
smach.Concurrence.add('find_faces',
recognize_face_concurrent())
smach.Concurrence.add('say_search_faces',
say_searching_faces())
smach.StateMachine.add('Concurrence',
sm_conc,
transitions={
'succeeded': 'DEFAULT_POSITION',
'aborted': 'Say_Finish',
'endTime': 'Say_Finish',
'preempted': 'Say_Finish'
})
smach.StateMachine.add('Say_Finish',
text_to_say(
'I have finish with no exit the search',
wait=False),
transitions={
'succeeded': 'DEFAULT_POSITION_NO',
'aborted': 'DEFAULT_POSITION_NO',
'preempted': 'preempted'
})
smach.StateMachine.add('DEFAULT_POSITION_NO',
move_head_form("center", "up"),
transitions={
'succeeded': 'aborted',
'aborted': 'aborted'
})
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center", "up"),
transitions={
'succeeded': 'succeeded',
'aborted': 'succeeded'
})
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,
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.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):
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'})
