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