def __init__(self, poi_name = None): smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['nav_to_poi_name'], output_keys=['standard_error']) with self: # We must initialize the userdata keys if they are going to be accessed or they won't exist and crash! self.userdata.standard_error='' self.userdata.nav_to_coord_goal=[0.0,0.0,0.0] smach.StateMachine.add('PrepareData', prepareData(poi_name), transitions={'succeeded':'translate_coord', 'aborted':'aborted'}) # We transform the poi to coordenates smach.StateMachine.add( 'translate_coord', translate_coord(), transitions={'succeeded': 'move_to_coord', 'aborted': 'aborted', 'preempted': 'aborted'}) # Go to coordanates smach.StateMachine.add( 'move_to_coord', nav_to_coord(), transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})
def __init__(self, yawPlus = 0,direction = "left"): smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=[], output_keys=['standard_error']) with self: # We must initialize the userdata keys if they are going to be accessed or they won't exist and crash! self.userdata.standard_error='' self.userdata.point_to_coord_goal=[0.0,0.0,0.0] # We get current coodenades of robot in userdata 'current_robot_pose', 'current_robot_yaw', 'pose_current' smach.StateMachine.add('get_pose', GetPoseSubscribe(), transitions={'succeeded': 'prepareNav', 'aborted': 'aborted', 'preempted': 'preempted'}) # Prepares to turn smach.StateMachine.add('prepareNav', prepare_nav_to_coord(yawPlus,direction), transitions={'succeeded': 'turn', 'aborted': 'aborted', 'preempted': 'preempted'}) # Turns smach.StateMachine.add('turn', nav_to_coord(), transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})
def __init__(self): smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'], output_keys=['person_location', 'person_location_coord']) with self: self.userdata.emergency_location = [] # Some dummy TTS stuff self.userdata.tts_lang = 'en_US' self.userdata.tts_wait_before_speaking = 0 smach.StateMachine.add( 'Prepare_Say_Searching', prepare_tts('Where are you? Give me signals, please.'), transitions={'succeeded':'Say_Search', 'aborted':'Say_Search', 'preempted':'Say_Search'}) smach.StateMachine.add( 'Say_Search', text_to_say(), transitions={'succeeded':'Gesture_Recognition', 'aborted':'Gesture_Recognition', 'preempted':'Gesture_Recognition'}) # Search for a Wave Gesture # Output_keys: gesture_detected: type Gesture self.userdata.nav_to_coord = [0, 0, 0] #Look Down #Move head right/left smach.StateMachine.add( 'Gesture_Recognition', WaveDetection(), transitions={'succeeded':'Say_Search','aborted':'Gesture_Recognition', 'preempted':'Gesture_Recognition'}) smach.StateMachine.add( 'Say_Search', text_to_say('Oh! I have found you.'), transitions={'succeeded':'Prepare_Go_To_Wave', 'aborted':'Prepare_Go_To_Wave', 'preempted':'Say_Search'}) smach.StateMachine.add( 'Prepare_Go_To_Wave', prepare_go_to_wave(), transitions={'succeeded':'Say_Go_to_Wave', 'aborted':'Say_Go_to_Wave', 'preempted':'Say_Go_to_Wave'}) smach.StateMachine.add( 'Say_Go_to_Wave', text_to_say("I'm coming!"), transitions={'succeeded':'Go_to_Wave', 'aborted':'Gesture_Recognition', 'preempted':'Gesture_Recognition'}) smach.StateMachine.add( 'Go_to_Wave', nav_to_coord('/base_link'), transitions={'succeeded':'Register_Position', 'aborted':'Go_to_Wave', 'preempted':'Go_to_Wave'}) smach.StateMachine.add( 'Register_Position', get_current_robot_pose(), transitions={'succeeded':'TreatPoseForCoord', 'aborted':'Register_Position', 'preempted':'Register_Position'}, remapping={'current_robot_pose':'person_location'}) smach.StateMachine.add( 'TreatPoseForCoord', PoseToArray(), transitions={'succeeded':'succeeded', 'aborted':'Register_Position', 'preempted':'Register_Position'})
def __init__(self, poi_name = None): smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['point_to_poi_name'], output_keys=['standard_error']) with self: # We must initialize the userdata keys if they are going to be accessed or they won't exist and crash! print "POINT TO POI IN-SELF" self.userdata.standard_error='' self.userdata.point_to_coord_goal=[0.0,0.0,0.0] smach.StateMachine.add('PrepareData', prepareData(poi_name), transitions={'succeeded':'translate_coord', 'aborted':'aborted'}) # We transform the poi to coordenades smach.StateMachine.add('translate_coord', translate_coord(), transitions={'succeeded': 'get_pose', 'aborted': 'aborted', 'preempted': 'preempted'}) # We get current coodenades of robot in userdata 'current_robot_pose', 'current_robot_yaw', 'pose_current' smach.StateMachine.add('get_pose', GetPoseSubscribe(), transitions={'succeeded': 'get_yaw', 'aborted': 'aborted', 'preempted': 'preempted'}) # We get current coodenades of robot in userdata 'current_robot_pose', 'current_robot_yaw', 'pose_current' smach.StateMachine.add('get_yaw', calculateYaw(), #output ['desired_angle'] transitions={'succeeded': 'prepareNav', 'aborted': 'aborted', 'preempted': 'preempted'}) # Prepares to turn smach.StateMachine.add('prepareNav', prepare_nav_to_coord(), transitions={'succeeded': 'turn', 'aborted': 'aborted', 'preempted': 'preempted'}) # Turns smach.StateMachine.add('turn', nav_to_coord(), transitions={'succeeded': 'point_to_coord', 'aborted': 'aborted', 'preempted': 'preempted'}) # Point the coordenades smach.StateMachine.add('point_to_coord', play_motion_sm('point_forward'), transitions={'succeeded': 'Say_Pointing_Poi', 'aborted': 'aborted', 'preempted': 'preempted'}) # Say pointing smach.StateMachine.add('Say_Pointing_Poi', text_to_say('There, what I am pointing'), transitions={'succeeded': 'home_position', 'aborted': 'aborted', 'preempted': 'preempted'}) # Return home position smach.StateMachine.add('home_position', play_motion_sm('home'), transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})
def __init__(self, yawPlus=0, direction="left"): smach.StateMachine.__init__( self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=[], output_keys=['standard_error']) with self: # We must initialize the userdata keys if they are going to be accessed or they won't exist and crash! self.userdata.standard_error = '' self.userdata.point_to_coord_goal = [0.0, 0.0, 0.0] # We get current coodenades of robot in userdata 'current_robot_pose', 'current_robot_yaw', 'pose_current' smach.StateMachine.add('get_pose', GetPoseSubscribe(), transitions={ 'succeeded': 'prepareNav', 'aborted': 'aborted', 'preempted': 'preempted' }) # Prepares to turn smach.StateMachine.add('prepareNav', prepare_nav_to_coord(yawPlus, direction), transitions={ 'succeeded': 'turn', 'aborted': 'aborted', 'preempted': 'preempted' }) # Turns smach.StateMachine.add('turn', nav_to_coord(), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted' })
def __init__(self, x = None, y = None, yaw = None, distance_to_human = DISTANCE_TO_HUMAN): smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['nav_to_coord_goal'], output_keys=['standard_error']) with self: # We must initialize the userdata keys if they are going to be accessed or they won't exist and crash! self.userdata.standard_error='' smach.StateMachine.add('PrepareData', prepareData(x, y, yaw, distance_to_human), transitions={'succeeded':'move_to_coord', 'aborted':'aborted'}) smach.StateMachine.add( 'move_to_coord', nav_to_coord(), transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})
def __init__(self, poi_name=None): smach.StateMachine.__init__( self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['nav_to_poi_name'], output_keys=['standard_error']) with self: # We must initialize the userdata keys if they are going to be accessed or they won't exist and crash! self.userdata.standard_error = '' self.userdata.nav_to_coord_goal = [0.0, 0.0, 0.0] smach.StateMachine.add('PrepareData', prepareData(poi_name), transitions={ 'succeeded': 'translate_coord', 'aborted': 'aborted' }) # We transform the poi to coordenates smach.StateMachine.add('translate_coord', translate_coord(), transitions={ 'succeeded': 'move_to_coord', 'aborted': 'aborted', 'preempted': 'aborted' }) # Go to coordanates smach.StateMachine.add('move_to_coord', nav_to_coord(), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted' })
def __init__(self): smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['distance'], output_keys=[]) with self: # Calculate the actual position smach.StateMachine.add( 'get_actual_pos', get_current_robot_pose(), transitions={'succeeded': 'prepare_move_base', 'aborted': 'aborted', 'preempted': 'succeeded'}) # Prepare the new goal smach.StateMachine.add( 'prepare_move_base', prepare_move_base(), transitions={'succeeded': 'go_to_point', 'aborted': 'aborted', 'preempted':'preempted'}) # Go to the new point smach.StateMachine.add( 'go_to_point', nav_to_coord(), 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): 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, 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'], output_keys=['standard_error'], input_keys=['in_learn_person']) with self: self.userdata.tts_wait_before_speaking=0 self.userdata.tts_text=None self.userdata.tts_lang=None self.userdata.standar_error="ok" self.userdata.word_to_listen=None self.userdata.in_learn_person=person() self.userdata.head_left_right=None self.userdata.head_up_down=None self.userdata.in_learn_person.targetId=1 print "HEY, THIS IS self.userdata.in_learn_person: " + str(self.userdata.in_learn_person) def go_back_request(userdata,request): start_request = NavigationGoBackRequest() start_request.enable=True start_request.meters=METERSBACK return start_request #maybe i will have to learn again smach.StateMachine.add('INIT_VAR', init_var(), transitions={'succeeded': 'GO_BACK', 'aborted': 'aborted','preempted':'preempted'}) # i stop the service def Cheack_Elevator_Stop(userdata, request): start_request = EnableCheckElevatorRequest() start_request.enable=False return start_request smach.StateMachine.add('STOP_CHECK_ELEVATOR', ServiceState('/check_elevator/enable', EnableCheckElevator, request_cb = Cheack_Elevator_Stop), transitions={'succeeded':'succeeded', 'preempted':'succeeded', 'aborted':'succeeded'}) # it will go from the lift smach.StateMachine.add('GO_BACK', ServiceState('/reverse', NavigationGoBack, request_cb = go_back_request), transitions={'succeeded':'READ_TRACKER_TOPIC','aborted' : 'READ_TRACKER_TOPIC','preempted':'preempted'}) # i don't understant why is this goal here, i imagine for stop smach.StateMachine.add('SEND_GOAL', nav_to_coord_concurrent('/base_link'), transitions={'succeeded':'READ_TRACKER_TOPIC', 'aborted':'WHERE_IS_IT','preempted':'preempted'}) smach.StateMachine.add('READ_TRACKER_TOPIC', topic_reader(topic_name='/pipol_tracker_node/peopleSet', topic_type=personArray,topic_time_out=60), transitions={'succeeded':'WHERE_IS_IT', 'aborted':'READ_TRACKER_TOPIC', 'preempted':'preempted'}, remapping={'topic_output_msg': 'tracking_msg'}) # it will have to look if it can find the person, know it's only one try smach.StateMachine.add('WHERE_IS_IT', where_is_it(), transitions={'i_dont_know':'SAY_COME_NEAR','ok_lets_go':'SAY_LETS_GO','preempted':'preempted'}) #this is when i don't remember the person smach.StateMachine.add('SAY_COME_NEAR', text_to_say(SAY_COME_NEAR), transitions={'succeeded':'LEARN_AGAIN','aborted':'LEARN_AGAIN','preempted':'preempted'}) #hear i will learn the peron another time smach.StateMachine.add('LEARN_AGAIN', LearnPerson(learn_face=False), transitions={'succeeded':'SAY_LETS_GO', 'aborted':'SAY_COME_NEAR','preempted':'preempted'}) smach.StateMachine.add('SAY_LETS_GO', text_to_say(SAY_LETS_GO), transitions={'succeeded':'TRACK_OPERATOR','aborted':'aborted','preempted':'preempted'}) smach.StateMachine.add('TRACK_OPERATOR', FollowOperator(learn_if_lost=False,feedback=True), transitions={'succeeded':'succeeded', 'lost':'LOST_CREATE_GOAL','preempted':'preempted'}) # i will prepere the navigation goal becouse i have lost the person #the goal have to be a realy far goal smach.StateMachine.add('LOST_CREATE_GOAL',create_nav_goal(), transitions={'succeeded':'SEARCH_OPERATOR', 'aborted':'SEARCH_OPERATOR','preempted':'preempted'}) sm=smach.Concurrence(outcomes=['gesture_recongize', 'nav_finisheed','preempted'], default_outcome='nav_finisheed', input_keys=["in_learn_person",'nav_to_coord_goal'], output_keys=['wave_position', 'wave_yaw_degree','standard_error'], child_termination_cb = child_term_cb, outcome_cb=out_cb_follow) with sm: sm.userdata.standar_error="ok" #this goal will be a realy far goal smach.Concurrence.add('SEND_GOAL', nav_to_coord("/base_link")) smach.Concurrence.add('SEARCH_OPERATOR_GESTURE', Search_Wave_SM()) smach.StateMachine.add('SEARCH_OPERATOR', sm, transitions={'gesture_recongize':'DEFAULT_POSITION', 'nav_finisheed':'SEARCH_OPERATOR_GESTURE_2','preempted':'preempted'}) smach.StateMachine.add('SEARCH_OPERATOR_GESTURE_2', Search_Wave_SM(), transitions={'succeeded':'DEFAULT_POSITION', 'aborted':'SEARCH_OPERATOR_GESTURE_2','preempted':'preempted', 'end_searching':'SEARCH_OPERATOR_GESTURE_2'}) smach.StateMachine.add('DEFAULT_POSITION', move_head_form("center","up"), transitions={'succeeded': 'CREATE_GESTURE_NAV_GOAL','aborted':'CREATE_GESTURE_NAV_GOAL'}) smach.StateMachine.add('CREATE_GESTURE_NAV_GOAL', create_nav_gesture_goal(), transitions={'succeeded':'RECOGNIZE_PERSON', 'aborted':'aborted','preempted':'preempted'}) # smach.StateMachine.add('SAY_CROSSING', # text_to_say(SAY_CROSSING), # transitions={'succeeded':'GO_TO_GESTURE', # 'aborted':'GO_TO_GESTURE','preempted':'preempted'}) # #if the navigation goal it's impossible it will be heare allways # smach.StateMachine.add('GO_TO_GESTURE', # nav_to_coord('/base_link'), # transitions={'succeeded':'RECOGNIZE_PERSON', # 'aborted':'RECOGNIZE_PERSON','preempted':'preempted'}) # #when i'm whit the person i have to look if it's the person smach.StateMachine.add('RECOGNIZE_PERSON', LearnPerson(learn_face=False), transitions={'succeeded':'SAY_GO_AGAIN', 'aborted':'aborted','preempted':'preempted'}) smach.StateMachine.add('SAY_GO_AGAIN', text_to_say(SAY_GO_AGAIN), transitions={'succeeded':'FOLLOW_AGAIN', 'aborted':'SEARCH_OPERATOR','preempted':'preempted'}) # hear i finish the state smach.StateMachine.add('FOLLOW_AGAIN', FollowOperator(feedback=True, learn_if_lost=True), transitions={'succeeded':'succeeded', 'lost':'aborted','preempted':'preempted'})
def __init__(self): smach.StateMachine.__init__( self, ['succeeded', 'preempted', 'aborted'], output_keys=['person_location', 'person_location_coord']) with self: self.userdata.emergency_location = [] # Some dummy TTS stuff self.userdata.tts_lang = 'en_US' self.userdata.tts_wait_before_speaking = 0 smach.StateMachine.add( 'Prepare_Say_Searching', prepare_tts('Where are you? Give me signals, please.'), transitions={ 'succeeded': 'Say_Search', 'aborted': 'Say_Search', 'preempted': 'Say_Search' }) smach.StateMachine.add('Say_Search', text_to_say(), transitions={ 'succeeded': 'Gesture_Recognition', 'aborted': 'Gesture_Recognition', 'preempted': 'Gesture_Recognition' }) # Search for a Wave Gesture # Output_keys: gesture_detected: type Gesture self.userdata.nav_to_coord = [0, 0, 0] #Look Down #Move head right/left smach.StateMachine.add('Gesture_Recognition', WaveDetection(), transitions={ 'succeeded': 'Say_Search', 'aborted': 'Gesture_Recognition', 'preempted': 'Gesture_Recognition' }) smach.StateMachine.add('Say_Search', text_to_say('Oh! I have found you.'), transitions={ 'succeeded': 'Prepare_Go_To_Wave', 'aborted': 'Prepare_Go_To_Wave', 'preempted': 'Say_Search' }) smach.StateMachine.add('Prepare_Go_To_Wave', prepare_go_to_wave(), transitions={ 'succeeded': 'Say_Go_to_Wave', 'aborted': 'Say_Go_to_Wave', 'preempted': 'Say_Go_to_Wave' }) smach.StateMachine.add('Say_Go_to_Wave', text_to_say("I'm coming!"), transitions={ 'succeeded': 'Go_to_Wave', 'aborted': 'Gesture_Recognition', 'preempted': 'Gesture_Recognition' }) smach.StateMachine.add('Go_to_Wave', nav_to_coord('/base_link'), transitions={ 'succeeded': 'Register_Position', 'aborted': 'Go_to_Wave', 'preempted': 'Go_to_Wave' }) smach.StateMachine.add( 'Register_Position', get_current_robot_pose(), transitions={ 'succeeded': 'TreatPoseForCoord', 'aborted': 'Register_Position', 'preempted': 'Register_Position' }, remapping={'current_robot_pose': 'person_location'}) smach.StateMachine.add('TreatPoseForCoord', PoseToArray(), transitions={ 'succeeded': 'succeeded', 'aborted': 'Register_Position', 'preempted': 'Register_Position' })
def __init__(self): smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted']) with self: # We must initialize the userdata keys if they are going to be accessed or they won't exist and crash! self.userdata.loop_iterations = 0 self.userdata.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, ['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'], 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, ['succeeded', 'preempted', 'aborted'], input_keys=['person_location', 'person_location_coord']) with self: self.userdata.emergency_location = [] self.userdata.tts_lang = 'en_US' self.userdata.tts_wait_before_speaking = 0 smach.StateMachine.add( 'Ask_Question', text_to_say(text='What would you like me to bring?'), transitions={ 'succeeded': 'Listen_Question', 'aborted': 'Ask_Question', 'preempted': 'Listen_Question' }) # Ask for the object to bring to the person # Output: # - string 'userSaid' # - actiontag[] 'asr_userSaid_tags' # TODO: grammar for the Emergency Situation -- Get_Person_Desired_Object smach.StateMachine.add('Listen_Question', ListenToSM(grammar='robocup/emergency'), transitions={ 'succeeded': 'Process_Tags', 'aborted': 'Ask_Question', 'preempted': 'Ask_Question' }) # Get the output from AskQuestionSM, process it, and search in the yaml file for the location of the object asked # Input keys: actiontag[] 'asr_userSaid_tags' # Output keys: object smach.StateMachine.add('Process_Tags', Process_Tags(), transitions={ 'succeeded': 'Say_go_Kitchen', 'aborted': 'Ask_Question', 'aborted': 'Ask_Question' }) smach.StateMachine.add( 'Say_go_Kitchen', text_to_say( 'I am Going to the Kitchen for an object, Stay Here until I give you the object' ), transitions={ 'succeeded': 'Go_To_Object_Place', 'aborted': 'Go_To_Object_Place', 'aborted': 'Go_To_Object_Place' }) smach.StateMachine.add('Go_To_Object_Place', nav_to_poi('kitchen'), transitions={ 'succeeded': 'Grasp_fail_Ask_Person', 'aborted': 'Grasp_fail_Ask_Person', 'preempted': 'Grasp_fail_Ask_Person' }) self.userdata.time_grasp = 0.0 smach.StateMachine.add('Grasping_with_timeout', grasping_with_timeout(), transitions={ 'succeeded': 'Prepare_Go_To_Person', 'time_out': 'Grasp_fail_Ask_Person' }) # sm_conc = smach.Concurrence(outcomes=['succeeded', 'time_out'], # default_outcome='succeeded', # input_keys=['object_to_grasp, time_grasp'], # child_termination_cb = child_term_cb, # outcome_cb = out_cb) # # with sm_conc: # sm_conc.add( # 'Find_and_grab_object', # #Find_and_grab_object(), # DummyStateMachine()) # sm_conc.add( # 'Time_State', # Time_State()) # # smach.StateMachine.add('GRASP_CONCURRENCE', # sm_conc, # transitions={'succeeded':'Prepare_Go_To_Person', # 'time_out':'Grasp_fail_Ask_Person'}) #Find Object + Grab Object SM # smach.StateMachine.add( # 'Find_and_grab_object', # #Find_and_grab_object(), # DummyStateMachine(), # transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'Grasp_fail_Ask_Person'}) smach.StateMachine.add('Grasp_fail_Ask_Person', ask_give_object_grasping(), transitions={ 'succeeded': 'Rest_arm', 'aborted': 'Rest_arm', 'preempted': 'Rest_arm' }) smach.StateMachine.add('Rest_arm', play_motion_sm('rest_object_right'), transitions={ 'succeeded': 'Prepare_Go_To_Person', 'aborted': 'Prepare_Go_To_Person', 'preempted': 'Prepare_Go_To_Person' }) #Go to person smach.StateMachine.add('Prepare_Go_To_Person', prepare_poi_person_emergency(), transitions={ 'succeeded': 'Go_To_Person', 'aborted': 'Go_To_Person', 'preempted': 'Go_To_Person' }) #TODO: POI For Person in Emergency -- From SearchPeople SM - smach.StateMachine.add( 'Go_To_Person', #DummyStateMachine(), #nav_to_poi(), nav_to_coord('/map'), transitions={ 'succeeded': 'Say_Give_Object', 'aborted': 'Say_Give_Object', 'preempted': 'Say_Give_Object' }) smach.StateMachine.add( 'Say_Give_Object', text_to_say('I am going to give you the Object you want.'), transitions={ 'succeeded': 'Give_object_arm', 'aborted': 'Give_object_arm', 'preempted': 'Give_object_arm' }) smach.StateMachine.add('Give_object_arm', play_motion_sm('give_object_right'), transitions={ 'succeeded': 'Give_Object', 'aborted': 'Give_Object', 'preempted': 'Give_Object' }) #Give the grabbed object to the person smach.StateMachine.add( 'Give_Object', #DummyStateMachine(), move_hands_form(hand_pose_name='pre_grasp', hand_side='right'), transitions={ 'succeeded': 'Give_Object_2', 'aborted': 'Give_Object', 'preempted': 'Give_Object' }) smach.StateMachine.add( 'Give_Object_2', #DummyStateMachine(), move_hands_form(hand_pose_name='full_open', hand_side='right'), transitions={ 'succeeded': 'Say_Rescue_stay', 'aborted': 'Give_Object_2', 'preempted': 'Give_Object_2' }) smach.StateMachine.add( 'Say_Rescue_stay', text_to_say( 'Please Stay here I am going to call for the Ambulance'), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'aborted': 'preempted' })
def __init__(self): smach.StateMachine.__init__( self, ['succeeded', 'preempted', 'aborted'], input_keys=['person_location', 'person_location_coord']) pose_place = PoseStamped() pose_place.header.frame_id = '/base_link' pose_place.pose.position.x = 0.0 pose_place.pose.position.y = 0.0 pose_place.pose.position.z = 1.0 with self: self.userdata.emergency_location = [] self.userdata.tts_lang = 'en_US' self.userdata.tts_wait_before_speaking = 0 self.userdata.object_failed = False self.userdata.object_name = None smach.StateMachine.add( 'Ask_Question', text_to_say(text='What would you like me to bring?'), transitions={ 'succeeded': 'Listen_Question', 'aborted': 'Ask_Question', 'preempted': 'Listen_Question' }) smach.StateMachine.add('Listen_Question', ListenToSM(grammar='robocup/emergency'), transitions={ 'succeeded': 'Process_Tags', 'aborted': 'Ask_Question', 'preempted': 'Ask_Question' }) # Get the output from AskQuestionSM, process it, and search in the yaml file for the location of the object asked # Input keys: actiontag[] 'asr_userSaid_tags' # Output keys: object smach.StateMachine.add('Process_Tags', Process_Tags(), transitions={ 'succeeded': 'Search_Object', 'aborted': 'Ask_Question', 'aborted': 'Ask_Question' }) smach.StateMachine.add( 'Search_Object', object_detection_and_grasping_sm(), transitions={ 'succeeded': 'Say_return_Person', 'fail_object_detection': 'Grasp_failed_prepare', 'fail_object_grasping': 'Grasp_failed_prepare', 'aborted': 'aborted', 'preempted': 'preempted' }, remapping={'object_name': 'object_to_grasp'}) smach.StateMachine.add( 'Grasp_failed_prepare', Fail_Detection_Grasping(), transitions={'succeeded': 'Grasp_fail_Ask_Person'}) # TODO: Saying to where to robot is heading. # smach.StateMachine.add( # 'Say_go_Place', # text_to_say('I am Going to the Kitchen for an object, Stay Here until I give you the object'), # transitions={'succeeded':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place'}) # smach.StateMachine.add( # 'Go_To_Object_Place', # nav_to_poi(), # transitions={'succeeded':'Say_got_to_Kitchen', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'Grasp_fail_Ask_Person'}) # smach.StateMachine.add( # 'Say_got_to_Kitchen', # text_to_say('I am in the Kitchen, I am going to grasp fail ask person'), # transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person'}) # # self.userdata.time_grasp = 0.0 # smach.StateMachine.add('Grasping_with_timeout', # grasping_with_timeout(), # transitions={'succeeded':'Prepare_Go_To_Person', 'time_out':'Grasp_fail_Ask_Person'}) smach.StateMachine.add('Grasp_fail_Ask_Person', ask_give_object_grasping(), transitions={ 'succeeded': 'Rest_arm', 'aborted': 'Rest_arm', 'preempted': 'Rest_arm' }) smach.StateMachine.add('Rest_arm', play_motion_sm('rest_object_right'), transitions={ 'succeeded': 'Say_return_Person', 'aborted': 'Say_return_Person', 'preempted': 'Say_return_Person' }) smach.StateMachine.add( 'Say_return_Person', text_to_say('I am preparing to go back to the person'), transitions={ 'succeeded': 'Prepare_Go_To_Person', 'aborted': 'Prepare_Go_To_Person', 'aborted': 'Prepare_Go_To_Person' }) #Go to person smach.StateMachine.add('Prepare_Go_To_Person', prepare_poi_person_emergency(), transitions={ 'succeeded': 'Go_To_Person', 'aborted': 'Go_To_Person', 'preempted': 'Go_To_Person' }) #TODO: POI For Person in Emergency -- From SearchPeople SM - smach.StateMachine.add('Go_To_Person', nav_to_coord('/map'), transitions={ 'succeeded': 'Say_Give_Object', 'aborted': 'Say_Give_Object', 'preempted': 'Say_Give_Object' }) smach.StateMachine.add( 'Say_Give_Object', text_to_say('I am going to give you the Object you asked.'), transitions={ 'succeeded': 'Select_next_state_grasping', 'aborted': 'Select_next_state_grasping', 'preempted': 'Select_next_state_grasping' }) smach.StateMachine.add('Select_next_state_grasping', Select_Grasp_Next_State(), transitions={ 'state_failed': 'Give_object_arm', 'state_succeeded': 'Give_object_both' }) smach.StateMachine.add('Give_object_both', place_object_sm(pose_place), transitions={ 'succeeded': 'Say_Rescue_stay', 'aborted': 'Say_Rescue_stay', 'preempted': 'preempted' }) smach.StateMachine.add('Give_object_arm', play_motion_sm('give_object_right'), transitions={ 'succeeded': 'Give_Object', 'aborted': 'Give_Object', 'preempted': 'Give_Object' }) #Give the grabbed object to the person smach.StateMachine.add('Give_Object', move_hands_form(hand_pose_name='pre_grasp', hand_side='right'), transitions={ 'succeeded': 'Give_Object_2', 'aborted': 'Give_Object', 'preempted': 'Give_Object' }) smach.StateMachine.add('Give_Object_2', move_hands_form(hand_pose_name='full_open', hand_side='right'), transitions={ 'succeeded': 'Say_Rescue_stay', 'aborted': 'Give_Object_2', 'preempted': 'Give_Object_2' }) smach.StateMachine.add( 'Say_Rescue_stay', text_to_say( 'Please Stay here I am going to call for the Ambulance'), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'aborted': '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.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'], input_keys=['person_location', 'person_location_coord']) with self: self.userdata.emergency_location = [] self.userdata.tts_lang = 'en_US' self.userdata.tts_wait_before_speaking = 0 smach.StateMachine.add( 'Ask_Question', text_to_say(text='What would you like me to bring?'), transitions={'succeeded':'Listen_Question', 'aborted': 'Ask_Question', 'preempted':'Listen_Question'}) # Ask for the object to bring to the person # Output: # - string 'userSaid' # - actiontag[] 'asr_userSaid_tags' # TODO: grammar for the Emergency Situation -- Get_Person_Desired_Object smach.StateMachine.add( 'Listen_Question', ListenToSM(grammar='robocup/emergency'), transitions={'succeeded':'Process_Tags', 'aborted':'Ask_Question', 'preempted':'Ask_Question'}) # Get the output from AskQuestionSM, process it, and search in the yaml file for the location of the object asked # Input keys: actiontag[] 'asr_userSaid_tags' # Output keys: object smach.StateMachine.add( 'Process_Tags', Process_Tags(), transitions={'succeeded':'Say_go_Kitchen', 'aborted':'Ask_Question', 'aborted':'Ask_Question'}) smach.StateMachine.add( 'Say_go_Kitchen', text_to_say('I am Going to the Kitchen for an object, Stay Here until I give you the object'), transitions={'succeeded':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place'}) smach.StateMachine.add( 'Go_To_Object_Place', nav_to_poi('kitchen'), transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'Grasp_fail_Ask_Person'}) self.userdata.time_grasp = 0.0 smach.StateMachine.add('Grasping_with_timeout', grasping_with_timeout(), transitions={'succeeded':'Prepare_Go_To_Person', 'time_out':'Grasp_fail_Ask_Person'}) # sm_conc = smach.Concurrence(outcomes=['succeeded', 'time_out'], # default_outcome='succeeded', # input_keys=['object_to_grasp, time_grasp'], # child_termination_cb = child_term_cb, # outcome_cb = out_cb) # # with sm_conc: # sm_conc.add( # 'Find_and_grab_object', # #Find_and_grab_object(), # DummyStateMachine()) # sm_conc.add( # 'Time_State', # Time_State()) # # smach.StateMachine.add('GRASP_CONCURRENCE', # sm_conc, # transitions={'succeeded':'Prepare_Go_To_Person', # 'time_out':'Grasp_fail_Ask_Person'}) #Find Object + Grab Object SM # smach.StateMachine.add( # 'Find_and_grab_object', # #Find_and_grab_object(), # DummyStateMachine(), # transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'Grasp_fail_Ask_Person'}) smach.StateMachine.add( 'Grasp_fail_Ask_Person', ask_give_object_grasping(), transitions={'succeeded':'Rest_arm', 'aborted':'Rest_arm', 'preempted':'Rest_arm'}) smach.StateMachine.add( 'Rest_arm', play_motion_sm('rest_object_right'), transitions={'succeeded':'Prepare_Go_To_Person', 'aborted':'Prepare_Go_To_Person', 'preempted':'Prepare_Go_To_Person'}) #Go to person smach.StateMachine.add( 'Prepare_Go_To_Person', prepare_poi_person_emergency(), transitions={'succeeded':'Go_To_Person', 'aborted':'Go_To_Person', 'preempted':'Go_To_Person'}) #TODO: POI For Person in Emergency -- From SearchPeople SM - smach.StateMachine.add( 'Go_To_Person', #DummyStateMachine(), #nav_to_poi(), nav_to_coord('/map'), transitions={'succeeded':'Say_Give_Object', 'aborted':'Say_Give_Object', 'preempted':'Say_Give_Object'}) smach.StateMachine.add( 'Say_Give_Object', text_to_say('I am going to give you the Object you want.'), transitions={'succeeded':'Give_object_arm', 'aborted':'Give_object_arm', 'preempted':'Give_object_arm'}) smach.StateMachine.add( 'Give_object_arm', play_motion_sm('give_object_right'), transitions={'succeeded':'Give_Object', 'aborted':'Give_Object', 'preempted':'Give_Object'}) #Give the grabbed object to the person smach.StateMachine.add( 'Give_Object', #DummyStateMachine(), move_hands_form(hand_pose_name='pre_grasp', hand_side='right'), transitions={'succeeded':'Give_Object_2', 'aborted':'Give_Object', 'preempted':'Give_Object'}) smach.StateMachine.add( 'Give_Object_2', #DummyStateMachine(), move_hands_form(hand_pose_name='full_open', hand_side='right'), transitions={'succeeded':'Say_Rescue_stay', 'aborted':'Give_Object_2', 'preempted':'Give_Object_2'}) smach.StateMachine.add( 'Say_Rescue_stay', text_to_say('Please Stay here I am going to call for the Ambulance'), transitions={'succeeded':'succeeded', 'aborted':'aborted', 'aborted':'preempted'})
def __init__(self, poi_name = None): smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['nav_to_poi_name'], output_keys=['standard_error']) with self: self.userdata.tts_text = None self.userdata.tts_wait_before_speaking = None self.userdata.tts_lang = None self.userdata.manip_motion_to_play = None self.userdata.manip_time_to_play = None self.userdata.skip_planning = False smach.StateMachine.add('PrepareData', prepareData(poi_name), transitions={'succeeded':'check_can_pass', 'aborted':'aborted'}) # Check door state smach.StateMachine.add('check_can_pass', check_door_status(), transitions={'succeeded': 'home_position', 'aborted': 'say_open_door', 'door_too_far': 'say_too_far_from_door'}) # Robot is too far from door smach.StateMachine.add( 'say_too_far_from_door', text_to_say("I'm too far from the door."), transitions={'succeeded': 'approach_to_door', 'aborted': 'approach_to_door'}) # Approach to the door smach.StateMachine.add( 'approach_to_door', nav_to_coord("/base_link"), transitions={'succeeded': 'check_can_pass', 'aborted': 'check_can_pass'}) # Robot ask to open the door smach.StateMachine.add( 'say_open_door', text_to_say("Can anyone open the door please?"), transitions={'succeeded': 'sleep_state', 'aborted': 'sleep_state'}) # Sleep time before speak smach.StateMachine.add( 'sleep_state', Sleeper(5), transitions={'succeeded': 'check_can_pass', 'aborted': 'check_can_pass'}) # Home position smach.StateMachine.add( 'home_position', play_motion_sm('home'), transitions={'succeeded': 'say_enter_room', 'aborted': 'say_enter_room', 'preempted': 'succeeded'}) # We don't need to prepare the state, it takes the input_key directly # Robot announces it is going through the door smach.StateMachine.add( 'say_enter_room', text_to_say("I am going to enter the room"), transitions={'succeeded': 'pass_door', 'aborted': 'pass_door'}) #We pass the door smach.StateMachine.add( 'pass_door', pass_door(), transitions={'succeeded': 'enter_room', 'aborted': 'pass_door', 'preempted': 'preempted'}) # Go to the poi in the other site of the door smach.StateMachine.add( 'enter_room', nav_to_poi(), transitions={'succeeded': 'succeeded', 'aborted': 'enter_room', 'preempted': 'preempted'})
def __init__(self): smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'], input_keys=['person_location', 'person_location_coord']) pose_place = PoseStamped() pose_place.header.frame_id = '/base_link' pose_place.pose.position.x = 0.0 pose_place.pose.position.y = 0.0 pose_place.pose.position.z = 1.0 with self: self.userdata.emergency_location = [] self.userdata.tts_lang = 'en_US' self.userdata.tts_wait_before_speaking = 0 self.userdata.object_failed = False self.userdata.object_name = None smach.StateMachine.add( 'Ask_Question', text_to_say(text='What would you like me to bring?'), transitions={'succeeded':'Listen_Question', 'aborted': 'Ask_Question', 'preempted':'Listen_Question'}) smach.StateMachine.add( 'Listen_Question', ListenToSM(grammar='robocup/emergency'), transitions={'succeeded':'Process_Tags', 'aborted':'Ask_Question', 'preempted':'Ask_Question'}) # Get the output from AskQuestionSM, process it, and search in the yaml file for the location of the object asked # Input keys: actiontag[] 'asr_userSaid_tags' # Output keys: object smach.StateMachine.add( 'Process_Tags', Process_Tags(), transitions={'succeeded':'Search_Object', 'aborted':'Ask_Question', 'aborted':'Ask_Question'}) smach.StateMachine.add( 'Search_Object', object_detection_and_grasping_sm(), transitions={'succeeded':'Say_return_Person', 'fail_object_detection':'Grasp_failed_prepare', 'fail_object_grasping':'Grasp_failed_prepare', 'aborted':'aborted', 'preempted':'preempted'}, remapping = {'object_name':'object_to_grasp'}) smach.StateMachine.add( 'Grasp_failed_prepare', Fail_Detection_Grasping(), transitions={'succeeded':'Grasp_fail_Ask_Person'}) # TODO: Saying to where to robot is heading. # smach.StateMachine.add( # 'Say_go_Place', # text_to_say('I am Going to the Kitchen for an object, Stay Here until I give you the object'), # transitions={'succeeded':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place'}) # smach.StateMachine.add( # 'Go_To_Object_Place', # nav_to_poi(), # transitions={'succeeded':'Say_got_to_Kitchen', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'Grasp_fail_Ask_Person'}) # smach.StateMachine.add( # 'Say_got_to_Kitchen', # text_to_say('I am in the Kitchen, I am going to grasp fail ask person'), # transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person'}) # # self.userdata.time_grasp = 0.0 # smach.StateMachine.add('Grasping_with_timeout', # grasping_with_timeout(), # transitions={'succeeded':'Prepare_Go_To_Person', 'time_out':'Grasp_fail_Ask_Person'}) smach.StateMachine.add( 'Grasp_fail_Ask_Person', ask_give_object_grasping(), transitions={'succeeded':'Rest_arm', 'aborted':'Rest_arm', 'preempted':'Rest_arm'}) smach.StateMachine.add( 'Rest_arm', play_motion_sm('rest_object_right'), transitions={'succeeded':'Say_return_Person', 'aborted':'Say_return_Person', 'preempted':'Say_return_Person'}) smach.StateMachine.add( 'Say_return_Person', text_to_say('I am preparing to go back to the person'), transitions={'succeeded':'Prepare_Go_To_Person', 'aborted':'Prepare_Go_To_Person', 'aborted':'Prepare_Go_To_Person'}) #Go to person smach.StateMachine.add( 'Prepare_Go_To_Person', prepare_poi_person_emergency(), transitions={'succeeded':'Go_To_Person', 'aborted':'Go_To_Person', 'preempted':'Go_To_Person'}) #TODO: POI For Person in Emergency -- From SearchPeople SM - smach.StateMachine.add( 'Go_To_Person', nav_to_coord('/map'), transitions={'succeeded':'Say_Give_Object', 'aborted':'Say_Give_Object', 'preempted':'Say_Give_Object'}) smach.StateMachine.add( 'Say_Give_Object', text_to_say('I am going to give you the Object you asked.'), transitions={'succeeded':'Select_next_state_grasping', 'aborted':'Select_next_state_grasping', 'preempted':'Select_next_state_grasping'}) smach.StateMachine.add( 'Select_next_state_grasping', Select_Grasp_Next_State(), transitions={'state_failed':'Give_object_arm', 'state_succeeded':'Give_object_both'}) smach.StateMachine.add( 'Give_object_both', place_object_sm(pose_place), transitions={'succeeded':'Say_Rescue_stay', 'aborted':'Say_Rescue_stay', 'preempted':'preempted'}) smach.StateMachine.add( 'Give_object_arm', play_motion_sm('give_object_right'), transitions={'succeeded':'Give_Object', 'aborted':'Give_Object', 'preempted':'Give_Object'}) #Give the grabbed object to the person smach.StateMachine.add( 'Give_Object', move_hands_form(hand_pose_name='pre_grasp', hand_side='right'), transitions={'succeeded':'Give_Object_2', 'aborted':'Give_Object', 'preempted':'Give_Object'}) smach.StateMachine.add( 'Give_Object_2', move_hands_form(hand_pose_name='full_open', hand_side='right'), transitions={'succeeded':'Say_Rescue_stay', 'aborted':'Give_Object_2', 'preempted':'Give_Object_2'}) smach.StateMachine.add( 'Say_Rescue_stay', text_to_say('Please Stay here I am going to call for the Ambulance'), transitions={'succeeded':'succeeded', 'aborted':'aborted', 'aborted':'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=['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, poi_name=None): smach.StateMachine.__init__( self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['nav_to_poi_name'], output_keys=['standard_error']) with self: self.userdata.tts_text = None self.userdata.tts_wait_before_speaking = None self.userdata.tts_lang = None self.userdata.manip_motion_to_play = None self.userdata.manip_time_to_play = None self.userdata.skip_planning = False smach.StateMachine.add('PrepareData', prepareData(poi_name), transitions={ 'succeeded': 'check_can_pass', 'aborted': 'aborted' }) # Check door state smach.StateMachine.add('check_can_pass', check_door_status(), transitions={ 'succeeded': 'home_position', 'aborted': 'say_open_door', 'door_too_far': 'say_too_far_from_door' }) # Robot is too far from door smach.StateMachine.add('say_too_far_from_door', text_to_say("I'm too far from the door."), transitions={ 'succeeded': 'approach_to_door', 'aborted': 'approach_to_door' }) # Approach to the door smach.StateMachine.add('approach_to_door', nav_to_coord("/base_link"), transitions={ 'succeeded': 'check_can_pass', 'aborted': 'check_can_pass' }) # Robot ask to open the door smach.StateMachine.add( 'say_open_door', text_to_say("Can anyone open the door please?"), transitions={ 'succeeded': 'sleep_state', 'aborted': 'sleep_state' }) # Sleep time before speak smach.StateMachine.add('sleep_state', Sleeper(5), transitions={ 'succeeded': 'check_can_pass', 'aborted': 'check_can_pass' }) # Home position smach.StateMachine.add('home_position', play_motion_sm('home'), transitions={ 'succeeded': 'say_enter_room', 'aborted': 'say_enter_room', 'preempted': 'succeeded' }) # We don't need to prepare the state, it takes the input_key directly # Robot announces it is going through the door smach.StateMachine.add('say_enter_room', text_to_say("I am going to enter the room"), transitions={ 'succeeded': 'pass_door', 'aborted': 'pass_door' }) #We pass the door smach.StateMachine.add('pass_door', pass_door(), transitions={ 'succeeded': 'enter_room', 'aborted': 'pass_door', 'preempted': 'preempted' }) # Go to the poi in the other site of the door smach.StateMachine.add('enter_room', nav_to_poi(), transitions={ 'succeeded': 'succeeded', 'aborted': 'enter_room', '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': '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.nav_to_poi_name='' # Params for drop faces - always will be the same self.userdata.name_database = "find_me_database" self.userdata.purgeAll = True # Init the state machine - Drop faces in the database smach.StateMachine.add( 'init_database', drop_faces(), remapping={'name':'name_database'}, transitions={'succeeded': 'save_face', 'aborted': 'aborted', 'preempted': 'preempted'}) # Save the data from the TC # We need one state machine that listen the name and then learn the face smach.StateMachine.add( 'save_face', SaveFaceSM(), transitions={'succeeded': 'prepare_location', 'aborted': 'aborted', 'preempted': 'preempted'}) # Prepare the info to go to the room smach.StateMachine.add( 'prepare_location', prepare_location(), transitions={'succeeded': 'go_location', 'aborted': 'aborted', 'preempted': 'preempted'}) # Go to the location smach.StateMachine.add( 'go_location', nav_to_poi(), transitions={'succeeded': 'search_faces', 'aborted': 'aborted', 'preempted': 'preempted'}) # Go around the room, it the robot find the TC return success smach.StateMachine.add( 'search_faces', SearchFacesSM(), transitions={'succeeded': 'prepare_coord_person', 'aborted': 'aborted', '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(), transitions={'succeeded': 'prepare_say_found', 'aborted': 'aborted', 'preempted': 'preempted'}) # Say "I found you!" smach.StateMachine.add( 'prepare_say_found', prepare_say_found(), transitions={'succeeded': 'say_found', 'aborted': 'aborted', 'preempted': 'preempted'}) smach.StateMachine.add( 'say_found', text_to_say(), transitions={'succeeded': 'gesture_recognition', 'aborted': 'aborted'}) # Recognize the direction smach.StateMachine.add( 'gesture_recognition', DummyStateMachine(), transitions={'succeeded': 'go_side', 'aborted': 'aborted', 'preempted': 'preempted'}) # Go to the direction smach.StateMachine.add( 'go_side', DummyStateMachine(), transitions={'succeeded': 'succeeded', 'aborted': 'aborted', '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): 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): smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'], output_keys=['standard_error'], input_keys=['in_learn_person']) with self: self.userdata.tts_wait_before_speaking = 0 self.userdata.tts_text = None self.userdata.tts_lang = None self.userdata.standar_error = "ok" self.userdata.word_to_listen = None self.userdata.in_learn_person = 1 def go_back_request(userdata, request): start_request = NavigationGoBackRequest() start_request.enable = True start_request.meters = METERSBACK return start_request #maybe i will have to learn again smach.StateMachine.add('INIT_VAR', init_var(), transitions={ 'succeeded': 'GO_BACK', 'aborted': 'aborted', 'preempted': 'preempted' }) def Cheack_Elevator_Stop(userdata, request): start_request = EnableCheckElevatorRequest() start_request.enable = False return start_request smach.StateMachine.add('STOP_CHECK_ELEVATOR', ServiceState( '/check_elevator/enable', EnableCheckElevator, request_cb=Cheack_Elevator_Stop), transitions={ 'succeeded': 'succeeded', 'preempted': 'succeeded', 'aborted': 'succeeded' }) # it will go from the lift smach.StateMachine.add('GO_BACK', ServiceState('/reverse', NavigationGoBack, request_cb=go_back_request), transitions={ 'succeeded': 'SEND_GOAL', 'aborted': 'SEND_GOAL', 'preempted': 'preempted' }) smach.StateMachine.add('SEND_GOAL', nav_to_coord_concurrent('/base_link'), transitions={ 'succeeded': 'WHERE_IS_IT', 'aborted': 'WHERE_IS_IT', 'preempted': 'preempted' }) # it will go out from the lift smach.StateMachine.add('WHERE_IS_IT', where_is_it(), transitions={ 'i_dont_know': 'SAY_COME_NEAR', 'ok_lets_go': 'SAY_LETS_GO', 'preempted': 'preempted' }) #this is when i don't remember the person smach.StateMachine.add('SAY_COME_NEAR', text_to_say(SAY_COME_NEAR), transitions={ 'succeeded': 'LEARN_AGAIN', 'aborted': 'LEARN_AGAIN', 'preempted': 'preempted' }) #hear i will learn the peron another time smach.StateMachine.add('LEARN_AGAIN', learn_again(), transitions={ 'succeeded': 'SAY_LETS_GO', 'aborted': 'SAY_COME_NEAR', 'preempted': 'preempted' }) smach.StateMachine.add('SAY_LETS_GO', text_to_say(SAY_LETS_GO), transitions={ 'succeeded': 'TRACK_OPERATOR', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add('TRACK_OPERATOR', FollowOperator(), transitions={ 'succeeded': 'succeeded', 'lost': 'LOST_CREATE_GOAL', 'preempted': 'preempted' }) # i will prepere the navigation goal becouse i have lost the person #the goal have to be a realy far goal smach.StateMachine.add('LOST_CREATE_GOAL', create_nav_goal(), transitions={ 'succeeded': 'SEARCH_OPERATOR', 'aborted': 'SEARCH_OPERATOR', 'preempted': 'preempted' }) sm = smach.Concurrence( outcomes=['gesture_recongize', 'nav_finisheed', 'preempted'], default_outcome='nav_finisheed', input_keys=["in_learn_person"], child_termination_cb=child_term_cb, outcome_cb=out_cb_follow, output_keys=[]) with sm: sm.userdata.standar_error = "ok" #this goal will be a realy far goal smach.Concurrence.add('SEND_GOAL', nav_to_coord()) smach.Concurrence.add('SEARCH_OPERATOR_GESTURE', gesture_recogniton()) smach.StateMachine.add('SEARCH_OPERATOR', sm, transitions={ 'gesture_recongize': 'CREATE_GESTURE_NAV_GOAL', 'nav_finisheed': 'SEARCH_OPERATOR', 'preempted': 'preempted' }) smach.StateMachine.add('CREATE_GESTURE_NAV_GOAL', create_nav_gesture_goal(), transitions={ 'succeeded': 'GO_TO_GESTURE', 'aborted': 'aborted', 'preempted': 'preempted' }) #if the navigation goal it's impossible it will be heare allways smach.StateMachine.add('GO_TO_GESTURE', nav_to_coord('/base_link'), transitions={ 'succeeded': 'RECOGNIZE_PERSON', 'aborted': 'RECOGNIZE_PERSON', 'preempted': 'preempted' }) #when i'm whit the person i have to look if it's the person smach.StateMachine.add('RECOGNIZE_PERSON', recognize_person(), transitions={ 'succeeded': 'SAY_GO_AGAIN', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add('SAY_GO_AGAIN', text_to_say(SAY_GO_AGAIN), transitions={ 'succeeded': 'FOLLOW_AGAIN', 'aborted': 'SEARCH_OPERATOR', 'preempted': 'preempted' }) # hear i finish the state smach.StateMachine.add('FOLLOW_AGAIN', FollowOperator(), transitions={ 'succeeded': 'succeeded', 'lost': 'aborted', 'preempted': 'preempted' })
def __init__(self): smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'], output_keys=['standard_error'], input_keys=['in_learn_person']) with self: self.userdata.tts_wait_before_speaking = 0 self.userdata.tts_text = None self.userdata.tts_lang = None self.userdata.standar_error = "ok" self.userdata.word_to_listen = None self.userdata.in_learn_person = person() self.userdata.head_left_right = None self.userdata.head_up_down = None self.userdata.in_learn_person.targetId = 1 print "HEY, THIS IS self.userdata.in_learn_person: " + str( self.userdata.in_learn_person) def go_back_request(userdata, request): start_request = NavigationGoBackRequest() start_request.enable = True start_request.meters = METERSBACK return start_request #maybe i will have to learn again smach.StateMachine.add('INIT_VAR', init_var(), transitions={ 'succeeded': 'GO_BACK', 'aborted': 'aborted', 'preempted': 'preempted' }) # i stop the service def Cheack_Elevator_Stop(userdata, request): start_request = EnableCheckElevatorRequest() start_request.enable = False return start_request smach.StateMachine.add('STOP_CHECK_ELEVATOR', ServiceState( '/check_elevator/enable', EnableCheckElevator, request_cb=Cheack_Elevator_Stop), transitions={ 'succeeded': 'succeeded', 'preempted': 'succeeded', 'aborted': 'succeeded' }) # it will go from the lift smach.StateMachine.add('GO_BACK', ServiceState('/reverse', NavigationGoBack, request_cb=go_back_request), transitions={ 'succeeded': 'READ_TRACKER_TOPIC', 'aborted': 'READ_TRACKER_TOPIC', 'preempted': 'preempted' }) # i don't understant why is this goal here, i imagine for stop smach.StateMachine.add('SEND_GOAL', nav_to_coord_concurrent('/base_link'), transitions={ 'succeeded': 'READ_TRACKER_TOPIC', 'aborted': 'WHERE_IS_IT', 'preempted': 'preempted' }) smach.StateMachine.add( 'READ_TRACKER_TOPIC', topic_reader(topic_name='/pipol_tracker_node/peopleSet', topic_type=personArray, topic_time_out=60), transitions={ 'succeeded': 'WHERE_IS_IT', 'aborted': 'READ_TRACKER_TOPIC', 'preempted': 'preempted' }, remapping={'topic_output_msg': 'tracking_msg'}) # it will have to look if it can find the person, know it's only one try smach.StateMachine.add('WHERE_IS_IT', where_is_it(), transitions={ 'i_dont_know': 'SAY_COME_NEAR', 'ok_lets_go': 'SAY_LETS_GO', 'preempted': 'preempted' }) #this is when i don't remember the person smach.StateMachine.add('SAY_COME_NEAR', text_to_say(SAY_COME_NEAR), transitions={ 'succeeded': 'LEARN_AGAIN', 'aborted': 'LEARN_AGAIN', 'preempted': 'preempted' }) #hear i will learn the peron another time smach.StateMachine.add('LEARN_AGAIN', LearnPerson(learn_face=False), transitions={ 'succeeded': 'SAY_LETS_GO', 'aborted': 'SAY_COME_NEAR', 'preempted': 'preempted' }) smach.StateMachine.add('SAY_LETS_GO', text_to_say(SAY_LETS_GO), transitions={ 'succeeded': 'TRACK_OPERATOR', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add('TRACK_OPERATOR', FollowOperator(learn_if_lost=False, feedback=True), transitions={ 'succeeded': 'succeeded', 'lost': 'LOST_CREATE_GOAL', 'preempted': 'preempted' }) # i will prepere the navigation goal becouse i have lost the person #the goal have to be a realy far goal smach.StateMachine.add('LOST_CREATE_GOAL', create_nav_goal(), transitions={ 'succeeded': 'SEARCH_OPERATOR', 'aborted': 'SEARCH_OPERATOR', 'preempted': 'preempted' }) sm = smach.Concurrence( outcomes=['gesture_recongize', 'nav_finisheed', 'preempted'], default_outcome='nav_finisheed', input_keys=["in_learn_person", 'nav_to_coord_goal'], output_keys=[ 'wave_position', 'wave_yaw_degree', 'standard_error' ], child_termination_cb=child_term_cb, outcome_cb=out_cb_follow) with sm: sm.userdata.standar_error = "ok" #this goal will be a realy far goal smach.Concurrence.add('SEND_GOAL', nav_to_coord("/base_link")) smach.Concurrence.add('SEARCH_OPERATOR_GESTURE', Search_Wave_SM()) smach.StateMachine.add('SEARCH_OPERATOR', sm, transitions={ 'gesture_recongize': 'DEFAULT_POSITION', 'nav_finisheed': 'SEARCH_OPERATOR_GESTURE_2', 'preempted': 'preempted' }) smach.StateMachine.add('SEARCH_OPERATOR_GESTURE_2', Search_Wave_SM(), transitions={ 'succeeded': 'DEFAULT_POSITION', 'aborted': 'SEARCH_OPERATOR_GESTURE_2', 'preempted': 'preempted', 'end_searching': 'SEARCH_OPERATOR_GESTURE_2' }) smach.StateMachine.add('DEFAULT_POSITION', move_head_form("center", "up"), transitions={ 'succeeded': 'CREATE_GESTURE_NAV_GOAL', 'aborted': 'CREATE_GESTURE_NAV_GOAL' }) smach.StateMachine.add('CREATE_GESTURE_NAV_GOAL', create_nav_gesture_goal(), transitions={ 'succeeded': 'RECOGNIZE_PERSON', 'aborted': 'aborted', 'preempted': 'preempted' }) # smach.StateMachine.add('SAY_CROSSING', # text_to_say(SAY_CROSSING), # transitions={'succeeded':'GO_TO_GESTURE', # 'aborted':'GO_TO_GESTURE','preempted':'preempted'}) # #if the navigation goal it's impossible it will be heare allways # smach.StateMachine.add('GO_TO_GESTURE', # nav_to_coord('/base_link'), # transitions={'succeeded':'RECOGNIZE_PERSON', # 'aborted':'RECOGNIZE_PERSON','preempted':'preempted'}) # #when i'm whit the person i have to look if it's the person smach.StateMachine.add('RECOGNIZE_PERSON', LearnPerson(learn_face=False), transitions={ 'succeeded': 'SAY_GO_AGAIN', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add('SAY_GO_AGAIN', text_to_say(SAY_GO_AGAIN), transitions={ 'succeeded': 'FOLLOW_AGAIN', 'aborted': 'SEARCH_OPERATOR', 'preempted': 'preempted' }) # hear i finish the state smach.StateMachine.add('FOLLOW_AGAIN', FollowOperator(feedback=True, learn_if_lost=True), transitions={ 'succeeded': 'succeeded', 'lost': 'aborted', 'preempted': 'preempted' })
def __init__(self, poi_name=None): smach.StateMachine.__init__( self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['point_to_poi_name'], output_keys=['standard_error']) with self: # We must initialize the userdata keys if they are going to be accessed or they won't exist and crash! print "POINT TO POI IN-SELF" self.userdata.standard_error = '' self.userdata.point_to_coord_goal = [0.0, 0.0, 0.0] smach.StateMachine.add('PrepareData', prepareData(poi_name), transitions={ 'succeeded': 'translate_coord', 'aborted': 'aborted' }) # We transform the poi to coordenades smach.StateMachine.add('translate_coord', translate_coord(), transitions={ 'succeeded': 'get_pose', 'aborted': 'aborted', 'preempted': 'preempted' }) # We get current coodenades of robot in userdata 'current_robot_pose', 'current_robot_yaw', 'pose_current' smach.StateMachine.add('get_pose', GetPoseSubscribe(), transitions={ 'succeeded': 'get_yaw', 'aborted': 'aborted', 'preempted': 'preempted' }) # We get current coodenades of robot in userdata 'current_robot_pose', 'current_robot_yaw', 'pose_current' smach.StateMachine.add( 'get_yaw', calculateYaw(), #output ['desired_angle'] transitions={ 'succeeded': 'prepareNav', 'aborted': 'aborted', 'preempted': 'preempted' }) # Prepares to turn smach.StateMachine.add('prepareNav', prepare_nav_to_coord(), transitions={ 'succeeded': 'turn', 'aborted': 'aborted', 'preempted': 'preempted' }) # Turns smach.StateMachine.add('turn', nav_to_coord(), transitions={ 'succeeded': 'point_to_coord', 'aborted': 'aborted', 'preempted': 'preempted' }) # Point the coordenades smach.StateMachine.add('point_to_coord', play_motion_sm('point_forward'), transitions={ 'succeeded': 'Say_Pointing_Poi', 'aborted': 'aborted', 'preempted': 'preempted' }) # Say pointing smach.StateMachine.add('Say_Pointing_Poi', text_to_say('There, what I am pointing'), transitions={ 'succeeded': 'home_position', 'aborted': 'aborted', 'preempted': 'preempted' }) # Return home position smach.StateMachine.add('home_position', play_motion_sm('home'), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted' })