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'})