Python say.text_to_say函数代码示例

    def __init__(self):
        smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
                                     input_keys=[],
                                     output_keys=[])

        with self:
            self.userdata.pressed = False
            self.userdata.press = False
            self.userdata.tts_time_before_speaking = 0
            self.userdata.tts_text = ""
            self.userdata.tts_lang = ""
            
            # Check if the button is pressed
            smach.StateMachine.add(
                'check_button',
                emergency_button(),
                transitions= {'succeeded':'check_button_pressed', 'aborted':'check_button', 'preempted':'preempted'})
            
            smach.StateMachine.add(
                'check_button_pressed',
                check_button_pressed(),
                transitions= {'pressed':'say_pressed_button', 'unpressed':'say_not_pressed_button',
                              'succeeded':'check_button', 'preempted':'preempted'})
            
            # Say button pressed
            smach.StateMachine.add(
                'say_pressed_button',
                text_to_say("Oh you pressed my emergency button, feel free to check me around"),
                transitions= {'succeeded':'check_button', 'aborted':'check_button', 'preempted':'preempted'})

            # Say button is not pressed 
            smach.StateMachine.add(
                'say_not_pressed_button',
                text_to_say("Thank you for unpressing my emergency button. I'll recover my status shortly."),
                transitions= {'succeeded':'succeeded', 'aborted':'succeeded', 'preempted':'preempted'})

    def __init__(self):
        smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'], output_keys=['standard_error'])
        with self:

            # We prepare the information to go to the init door
            smach.StateMachine.add(
                'prepare_Avoid',
                prepare_Avoid(),
                transitions={'succeeded': 'say_go_to_poi', 'aborted': 'aborted', 
                'preempted': 'preempted'})  
            
            # Announce going to a place
            smach.StateMachine.add(
                    'say_go_to_poi',
                    text_to_say(text="I'm going to the Avoid That area."),
                    transitions={'succeeded': 'go_to_poi'})

            # Go to the POI
            smach.StateMachine.add(
                'go_to_poi',
                nav_to_poi(),
                transitions={'succeeded': 'say_get_to_poi', 'aborted': 'aborted', 
                'preempted': 'preempted'})    
            
            # Announce arriving to a place
            smach.StateMachine.add(
                    'say_get_to_poi',
                    text_to_say(text="I've arrived to the destination for Avoid That. The test Avoid That has finished successfully. Thank you."),
                    transitions={'succeeded': 'succeeded'})

    def __init__(self):
        smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
                                    output_keys=['standard_error','in_learn_person'])

        with self:
            self.userdata.tts_wait_before_speaking=0
            self.userdata.tts_text=None
            self.userdata.tts_lang=None
            self.userdata.standard_error='OK'
            self.userdata.in_learn_person=1
            smach.StateMachine.add('INTRO',
                                   text_to_say(START_FRASE),
                                   transitions={'succeeded': 'Listen','aborted':'aborted'})

            smach.StateMachine.add('Listen',
                                   ListenWordSM("follow me"),
                                   transitions={'succeeded': 'START_FOLLOWING_COME_CLOSER',
                                                'aborted': 'Listen'})
          
            smach.StateMachine.add('START_FOLLOWING_COME_CLOSER',
                                   text_to_say(START_FOLLOW_FRASE),
                                   transitions={'succeeded': 'SM_LEARN_PERSON','aborted':'aborted'})

            # it learns the person that we have to follow
            smach.StateMachine.add('SM_LEARN_PERSON',
                                   LearnPerson(),
                                   transitions={'succeeded': 'SM_STOP_LEARNING',
                                                'aborted': 'aborted'})

            smach.StateMachine.add('SM_STOP_LEARNING',
                                   text_to_say(LEARNED_PERSON_FRASE),
                                   transitions={'succeeded': 'succeeded','aborted':'aborted'})

    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,learn_face=False):
        smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
                                    output_keys=['standard_error','in_learn_person'])
        self.learn_face = learn_face
        self.follow_pub = rospy.Publisher('/follow_me/id', Int32, latch=True)    
        
        with self:
            self.userdata.tts_wait_before_speaking=0
            self.userdata.tts_text=None
            self.userdata.tts_lang=None
            self.userdata.standard_error='OK'
            


            smach.StateMachine.add('INIT_VAR',
                                   init_var(),
                                   transitions={'succeeded': 'READ_TRACKER_TOPIC',
                                                'aborted': 'SAY_LIFTIME'})
            smach.StateMachine.add('SAY_LIFTIME',
                                   text_to_say(SAY_GO_MIDLE),
                                   transitions={'succeeded': 'CONTROL_TIME',
                                                'aborted': 'SAY_LIFTIME'})

            smach.StateMachine.add('CONTROL_TIME',
                                   control_time(),
                                   transitions={'succeeded': 'READ_TRACKER_TOPIC',
                                                'lif_time': 'SAY_LIFTIME'})

#TODO:: aborted->CONTROL_TIME
            smach.StateMachine.add('READ_TRACKER_TOPIC',
                                   topic_reader(topic_name='/pipol_tracker_node/peopleSet',
                                                topic_type=personArray,topic_time_out=60),
                                   transitions={'succeeded':'SELECT_ID',
                                                'aborted':'READ_TRACKER_TOPIC',
                                                'preempted':'preempted'},
                                   remapping={'topic_output_msg': 'tracking_msg'})
           
            
           
            # it learns the person that we have to follow
            smach.StateMachine.add('SELECT_ID',
                                   select_ID(self.learn_face, self.follow_pub),
                                   transitions={'succeeded': 'STOP_LEARNING',
                                                'aborted': 'CONTROL_TIME','learn_face':'FACE_SAY'})
            
            smach.StateMachine.add('FACE_SAY',
                                   text_to_say(SAY_LEARN),
                                   transitions={'succeeded': 'LEARN_FACE','aborted':'LEARN_FACE'})
                        # it learns the person that we have to follow
            smach.StateMachine.add('LEARN_FACE',
                                   new_database_and_learn(learning_time=2, minConfidence=80, 
                                                          name_face="following", name_database="follow"),
                                   transitions={'succeeded': 'STOP_LEARNING',
                                                'aborted': 'CONTROL_TIME'})
            

            smach.StateMachine.add('STOP_LEARNING',
                                   text_to_say(SAY_FINISH),
                                   transitions={'succeeded': 'succeeded','aborted':'STOP_LEARNING'})

    def __init__(self):
        smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['object_array'])

        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=None
            self.userdata.standard_error='OK'
            self.userdata.object_name = ''
            self.userdata.object_index = 0

            # Process order
            smach.StateMachine.add(
                'decide_next_object',
                decide_next_object(),
                transitions={'succeeded': 'go_to_object', 'aborted': 'aborted', 
                'preempted': 'preempted'}) 
            
            # Go to poi where the object can stay
            smach.StateMachine.add(
                'go_to_object',
                nav_to_poi(),
                remapping={"nav_to_poi_name": "object_loc"},
                transitions={'succeeded': 'object_detection', 'aborted': 'aborted'})
                        
            # Object Detection
            smach.StateMachine.add(
                'object_detection',
                recognize_object(),
                transitions={'succeeded': 'grasp_object', 'aborted': 'aborted'})

            # Grasp Object
            smach.StateMachine.add(
                'grasp_object',
                text_to_say("Grasping Object"),
                transitions={'succeeded': 'go_to_delivery', 'aborted': 'aborted', 
                'preempted': 'preempted'}) 
            
            # Go to the delivery place
            smach.StateMachine.add(
                'go_to_delivery',
                nav_to_poi(),
                remapping = {'nav_to_poi_name': 'delivery_loc'},
                transitions={'succeeded': 'deliver_object', 'aborted': 'aborted', 
                'preempted': 'preempted'}) 
                        
            # Deliver object
            smach.StateMachine.add(
                'deliver_object',
                text_to_say("Delivering Object"),
                transitions={'succeeded': 'check_loop', 'aborted': 'aborted', 
                'preempted': 'preempted'}) 
            
            # End of loop?
            smach.StateMachine.add(
                'check_loop',
                checkLoop(),
                transitions={'succeeded': 'decide_next_object', 'aborted': 'aborted', 
                'preempted': 'preempted', 'end':'succeeded'}) 

    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 = 1
            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(
                'play_motion_state',
                play_motion_sm('home', skip_planning=True),
                transitions={'succeeded': 'init_cocktail',
                             'preempted':'init_cocktail', 
                             'aborted':'play_motion_state'})   
            
            smach.StateMachine.add(
                 'init_cocktail',
                 text_to_say("Ready for cocktail party"),
                 transitions={'succeeded': 'Ask_order', 'aborted': 'Ask_order'}) 
                  
            # We wait for open door and go inside
            smach.StateMachine.add(
                 'wait_for_door',
                 EnterRoomSM("party_room"),
                 transitions={'succeeded': 'Ask_order', 'aborted': 'aborted', 'preempted': 'preempted'}) 
               
            # Ask Order -> Wave + Learn Person + Order
            smach.StateMachine.add(
                'Ask_order',
                AskAllOrders(),
                transitions={'succeeded':'execute_order', 'aborted':'aborted'})   
            
#             # Go to the storage_room
#             smach.StateMachine.add(
#                 'go_to_storage',
#                 nav_to_poi("storage_room"),
#                 transitions={'succeeded': 'execute_order', 'aborted': 'go_to_storage', 
#                 'preempted': 'preempted'}) 
            
            # Execute the order 
            smach.StateMachine.add(
                'execute_order',
                ExecuteAllOrders(),
                transitions={'succeeded': 'say_leaving_arena', 'aborted': 'aborted', 
                'preempted': 'preempted'}) 
            
            # Say leaving the arena 
            smach.StateMachine.add(
                'say_leaving_arena',
                text_to_say("I finished the cocktail party, I'm leaving the arena", wait=False),
                transitions={'succeeded': 'succeeded', 'aborted': 'succeeded', 
                'preempted': 'preempted'})             

    def __init__(self,time_enroll=10):
        smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
                                        input_keys=['name_face','delete_database'],
                                        output_keys=['name_face'])

        with self:
            self.userdata.name_face = ''
            self.userdata.asr_userSaid = ''
            self.userdata.grammar_name = ''
            
            # Ask for name
            smach.StateMachine.add(
                'listen_name',
                AskQuestionSM("Hi, what's your name?", GRAMMAR_NAME),
                transitions={'succeeded': 'prepare_name', 'aborted': 'ask_name_again', 
                'preempted': 'preempted'}) 
            
            # We prepare the name for face_detection 
            smach.StateMachine.add(
                "prepare_name",
                prepare_name(),
                transitions={'succeeded': 'say_start', 'aborted': 'ask_name_again', 
                'preempted': 'preempted'})  
            
            # Ask for name again
            smach.StateMachine.add(
                'ask_name_again',
                AskQuestionSM("Sorry, I don't understand you. Can you repeat your name, please?", GRAMMAR_NAME),
                transitions={'succeeded': 'prepare_name', 'aborted': 'aborted', 
                'preempted': 'preempted'}) 
           
            smach.StateMachine.add(
                'say_start',
                text_to_say("OK,  now i am going to enroll your  face, don't  move"),
                transitions={'succeeded': 'learn_face', 'aborted': 'aborted', 
                'preempted': 'preempted'}) 
            

            # Start learning
            smach.StateMachine.add(
                'learn_face',
                 new_database_and_learn(time_enroll),
                 transitions={'succeeded': 'prepare_say_name', 'aborted': 'aborted', 
                'preempted': 'preempted'}) 
            
            # Say learn name
            smach.StateMachine.add(
                'prepare_say_name',
                prepare_say_name(),
                transitions={'succeeded': 'say_name', 'aborted': 'aborted', 
                'preempted': 'preempted'}) 
            
            smach.StateMachine.add(
                'say_name',
                text_to_say(),
                transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 
                'preempted': 'preempted'}) 

        def __init__(self, Type, objectName, GRAMMAR_NAME='robocup/locations', command_key='finn', command_value='xxx'):
                smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
                
                self.userdata.dataType = Type
                self.userdata.object_name = objectName
                self.userdata.location_name = ''
                self.userdata.grammar_name = GRAMMAR_NAME
                self.userdata.tts_wait_before_speaking = 0
                self.userdata.tts_text = ''
                self.userdata.tts_lang = ''
                
                with self:

                        smach.StateMachine.add('ENABLE_GRAMMAR',
                                               ActivateASR(GRAMMAR_NAME),
                                               transitions={'succeeded': 'ASK_LOCATION'})
                        
                        smach.StateMachine.add('ASK_LOCATION',
                                                text_to_say("I don't know where the " + self.userdata.object_name + ' is. Do you know where could I find it?'),
                                                transitions={'succeeded': 'HEAR_COMMAND', 'aborted': 'aborted'})

                        smach.StateMachine.add('HEAR_COMMAND',
                                               ReadASR(),
                                               transitions={'aborted': 'HEAR_COMMAND', 'succeeded': 'BRING_LOCATION', 'preempted': 'preempted'},
                                               remapping={'asr_userSaid': 'userSaidData', 'asr_userSaid_tags':'userSaidTags'})
                        
                        smach.StateMachine.add('BRING_LOCATION',
                                               BringOrderLoc(),
                                               transitions={'aborted': 'HEAR_COMMAND', 'succeeded': 'PREPARATION_CONFIRM_OBJECT', 'preempted': 'preempted'},
                                               remapping={'location_name': 'location_name'})

                        smach.StateMachine.add('PREPARATION_CONFIRM_OBJECT',
                                               prepare_confirm_info(),
                                               transitions={'succeeded': 'CONFIRM_OBJECT'},
                                               remapping={'tosay':'tts_text'})
                        
                        smach.StateMachine.add('CONFIRM_OBJECT', 
                                               text_to_say(),
                                               transitions={'succeeded': 'DISABLE_GRAMMAR', 'aborted': 'DISABLE_GRAMMAR'})     
                         
                        smach.StateMachine.add('RECOGNIZE_COMMAND', 
                                                RecognizeCommand(command_key, command_value),
                                                transitions={'notvalid_command': 'NOT_VALID_COMMAND',
                                                'valid_command': 'VALID_COMMAND', 'preempted': 'preempted', 'aborted': 'aborted'},
                                                remapping={'speechData': 'userSaidData'})
 
                        smach.StateMachine.add('VALID_COMMAND',
                                               text_to_say("Ok, understood."),
                                               transitions={'succeeded': 'DISABLE_GRAMMAR'})
 
                        smach.StateMachine.add('NOT_VALID_COMMAND',
                                               text_to_say("I couldn't understand what you said. Can you repeat?"),
                                               transitions={'succeeded': 'HEAR_COMMAND'})
 
                        smach.StateMachine.add('DISABLE_GRAMMAR',
                                               DeactivateASR(),
                                               transitions={'succeeded': 'succeeded'})

    def __init__(self):
        smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted', 'fail_grasp', 'fail_recognize'],
                                     input_keys=['object_name'], 
                                     output_keys=['object_position','object_detected_name'])

        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=''
            self.userdata.tts_lang = ''
            self.userdata.tts_wait_before_speak = ''
            self.userdata.tts_text = ''
                        
            # Say start object recognition
            smach.StateMachine.add(
                 'say_start_obj_recognition',
                 text_to_say("I'm going to start the Object recognition process.", wait=False),
                 transitions={'succeeded': 'object_recognition', 'aborted': 'object_recognition'}) 
             
            # Do object_recognition 
            smach.StateMachine.add(
                'object_recognition',
                recognize_object(),
                transitions={'succeeded': 'process_object_recognition', 'aborted': 'fail_recognize', 
                'preempted': 'preempted'}) 
   
            # Process the objects recognized
            smach.StateMachine.add(
                'process_object_recognition',
                process_pick_location(),
                transitions={'succeeded': 'say_grasp_object', 'aborted': 'fail_recognize', 
                'preempted': 'preempted'}) 

            # Say grasp object
            smach.StateMachine.add(
                 'say_grasp_object',
                 text_to_say("I'm going to grasp the object", wait=False),
                 transitions={'succeeded': 'grasp_object', 'aborted': 'aborted'})
            
            # Grasp the object
            smach.StateMachine.add(
                'grasp_object',
                pick_object_sm(),
                transitions={'succeeded': 'succeeded', 'aborted': 'home_position',
                'preempted': 'preempted'}) 
                     
            # Home position
            smach.StateMachine.add(
                'home_position',
                play_motion_sm('home',skip_planning=True),
                transitions={'succeeded': 'fail_grasp', 'aborted': 'home_position', 
                'preempted': 'preempted'}) 

    def __init__(self, grammar = None):
        smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
                                        input_keys=['grammar_name'],
                                        output_keys=['asr_userSaid','asr_userSaid_tags'])

        with self:
            self.userdata.asr_userSaid = ''
            self.userdata.tts_lang = None
            asr_userSaid_tags=''
            
            smach.StateMachine.add('PrepareData',
                    prepareData(grammar),
                    transitions={'succeeded':'listen_info', 'aborted':'aborted'})
             
            # Listen             
            smach.StateMachine.add(
                'listen_info',
                ListenToSM(),
                transitions={'succeeded': 'prepare_info', 'aborted': 'aborted', 
                'preempted': 'preempted'}) 
            
            # Prepare the information 
            smach.StateMachine.add(
                "prepare_info",
                prepare_say(),
                transitions={'succeeded': 'say_info', 'aborted': 'aborted', 
                'preempted': 'preempted'})  
           
            # Repeat
            smach.StateMachine.add(
                'say_info',
                text_to_say(),
                transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 
                'preempted': 'preempted'}) 

    def __init__(self):
        smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'], 
                                    input_keys=['object_to_grasp'],
                                    output_keys=['standard_error'])
        with self:
            # Preparation of the Ask Process
            # Input Data: 'object_to_grasp'
            self.userdata.tts_wait_before_speaking = 0
            self.userdata.tts_lang = 'en_US'
            smach.StateMachine.add('Prepare_Ask_Person_Object',
                                    prepare_ask_person_object(),
                                    transitions={'succeeded':'Ask_Person_Object', 'preempted':'Ask_Person_Object', 'aborted':'Ask_Person_Object'})

            smach.StateMachine.add('Ask_Person_Object',
                                    text_to_say(), 
                                    transitions={'succeeded':'Reach_Arm', 'preempted':'Reach_Arm', 'aborted':'Reach_Arm'})
            
            # Reach the arm
            self.userdata.manip_motion_to_play = 'give_object_right'
            smach.StateMachine.add('Reach_Arm',
                                    play_motion_sm(),
                                    transitions={'succeeded':'Pre_Grasp', 'preempted':'Reach_Arm', 'aborted':'Reach_Arm'})
    

            smach.StateMachine.add('Pre_Grasp',
                                    move_hands_form(hand_pose_name='pre_grasp', hand_side='right'),
                                    transitions={'succeeded':'Full_Grasp', 'preempted':'Full_Grasp', 'aborted':'Full_Grasp'})
            smach.StateMachine.add('Full_Grasp',
                                    move_hands_form(hand_pose_name='grasp', hand_side='right'),
                                    transitions={'succeeded':'succeeded', 'preempted':'preempted', 'aborted':'aborted'})

    def __init__(self):
        smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
                                    output_keys=['standard_error'])
        with self:
            # Preparation of the Ask Process
            # Input Data: 'object_to_grasp'
            self.userdata.tts_wait_before_speaking = 0
            self.userdata.tts_lang = 'en_US'
            smach.StateMachine.add('Ask_Person_Object',
                                    text_to_say('I am giving you the object you asked'), 
                                    transitions={'succeeded':'Reach_Arm', 'preempted':'Reach_Arm', 'aborted':'Reach_Arm'})
            
            # Reach the arm
            self.userdata.manip_motion_to_play = 'give_object_right'
            self.userdata.manip_time_to_play = 2.0
            smach.StateMachine.add('Reach_Arm',
                                    play_motion_sm(),
                                    transitions={'succeeded':'Pre_Grasp', 'preempted':'Reach_Arm', 'aborted':'Reach_Arm'})

            smach.StateMachine.add('Pre_Grasp',
                                    move_hands_form(hand_pose_name='pre_grasp', hand_side='right'),
                                    transitions={'succeeded':'Open_Hand', 'preempted':'Open_Hand', 'aborted':'Open_Hand'})
            smach.StateMachine.add('Open_Hand',
                                    move_hands_form(hand_pose_name='full_open', hand_side='right'),
                                    transitions={'succeeded':'Home_position_end', 'preempted':'Home_position_end', 'aborted':'Home_position_end'})
                            
            smach.StateMachine.add('Home_position_end',
                                    play_motion_sm("home", 4.0),
                                    transitions={'succeeded':'succeeded', 'preempted':'preempted', 'aborted':'aborted'})

 def __init__(self, head_position=None):
     smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted', 'end_searching'],
                              input_keys=[],
                              output_keys=['wave_position', 'wave_yaw_degree','standard_error'])
     with self:
         self.userdata.loop_iterations = 0
         self.userdata.wave_position = None
         self.userdata.wave_yaw_degree = None
         smach.StateMachine.add(
                                'Move_head_prepare',
                                prepare_move_head(),
                                 transitions={'succeeded': 'move_head', 'aborted': 'aborted', 
                                             'preempted': 'preempted', 'end_searching':'end_searching'})
         smach.StateMachine.add(
                                'move_head',
                                move_head_form(head_up_down='normal'),
                                transitions={'succeeded': 'Say_Searching',
                                             'preempted':'Say_Searching',
                                             'aborted':'aborted'})
         smach.StateMachine.add(
             'Say_Searching',
             text_to_say("I am searching, let's see"),
             transitions={'succeeded':'wave_recognition', 'aborted':'wave_recognition', 'preempted':'wave_recognition'})
        
         smach.StateMachine.add(
             'wave_recognition',
             WaveDetection(),
             transitions={'succeeded': 'succeeded', 'aborted': 'Move_head_prepare', 
             'preempted': 'preempted'}) 

    def __init__(self):
        smach.StateMachine.__init__(self, outcomes=['succeeded','aborted', 'preempted'], 
            input_keys=[], 
            output_keys=[])

        with self:

            smach.StateMachine.add('Wait_search_1', 
                                   Wait_search(), 
                                   transitions={'succeeded':'Say_search', 'preempted':'preempted'})
            
            smach.StateMachine.add('Say_search', 
                                   text_to_say('I am looking for the referee',wait=True), 
                                   transitions={'succeeded':'Move_head_Left', 'aborted':'aborted'})        
            smach.StateMachine.add('Move_head_Left',
                                   look_to_point(direction="left", min_duration=3),
                                   transitions={'succeeded':'Wait_search_2', 'aborted':'aborted'})
             
            smach.StateMachine.add('Wait_search_2', 
                                   Wait_search(), 
                                   transitions={'succeeded':'Move_head_Right', 'preempted':'preempted'})
             
            smach.StateMachine.add('Move_head_Right',
                                   look_to_point(direction="front", min_duration=2),
                                   transitions={'succeeded':'Wait_search_3', 'aborted':'aborted'})
             
            smach.StateMachine.add('Wait_search_3', 
                                   Wait_search(), 
                                   transitions={'succeeded':'Move_head_Middle', 'preempted':'preempted'})
             
            smach.StateMachine.add('Move_head_Middle',
                                   look_to_point(direction="right", min_duration=2),
                                   transitions={'succeeded':'Wait_search_1', 'aborted':'Wait_search_1'})