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=['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'})
Exemple #3
0
    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 my destiny"),
                                   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 my destiny"),
                transitions={'succeeded': 'succeeded'})
Exemple #4
0
    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': 'Learn',
                                       'aborted': 'aborted'
                                   })

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

            smach.StateMachine.add('SM_STOP_LEARNING',
                                   text_to_say(LETS_GO),
                                   transitions={
                                       'succeeded': 'succeeded',
                                       'aborted': 'aborted'
                                   })
    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=['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, ['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):
        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'}) 
Exemple #10
0
    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.head_left_right = None
            self.userdata.head_up_down = None
            self.userdata.standard_error = 'OK'
            self.userdata.in_learn_person = 1

            smach.StateMachine.add('DEFAULT_POSITION',
                                   move_head_form("center", "up"),
                                   transitions={
                                       'succeeded': 'INTRO',
                                       'aborted': 'INTRO'
                                   })

            smach.StateMachine.add('INTRO',
                                   text_to_say(START_FRASE),
                                   transitions={
                                       'succeeded':
                                       'START_FOLLOWING_COME_CLOSER',
                                       'aborted': 'START_FOLLOWING_COME_CLOSER'
                                   })

            smach.StateMachine.add('Listen',
                                   ListenWordSM_Concurrent("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(learn_face=True),
                                   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=['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'

            smach.StateMachine.add('INIT_VAR',
                                   init_var(),
                                   transitions={
                                       'succeeded': 'SAY_LIFTIME',
                                       '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': 'INIT_VAR'
                                   })

            smach.StateMachine.add(
                'READ_TRACKER_TOPIC',
                topic_reader(topic_name='/people_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(),
                                   transitions={
                                       'succeeded': 'STOP_LEARNING',
                                       'aborted': 'CONTROL_TIME'
                                   })

            smach.StateMachine.add('STOP_LEARNING',
                                   text_to_say("OK i have finsished"),
                                   transitions={
                                       'succeeded': 'succeeded',
                                       'aborted': 'STOP_LEARNING'
                                   })
    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'}) 
Exemple #13
0
def main():
    rospy.init_node('yes_no_test')

    sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])

    with sm:
        sm.userdata.grammar_name = None
        sm.userdata.tts_text = None
        sm.userdata.tts_wait_before_speaking = None
        sm.userdata.tts_lang = None

        # Load grammar yes/no
        smach.StateMachine.add('ActivateASR_yesno',
                               ActivateASR("robocup/yes_no"),
                               transitions={
                                   'succeeded': 'DID_YOU_SAY',
                                   'aborted': 'aborted',
                                   'preempted': 'preempted'
                               })

        smach.StateMachine.add('DID_YOU_SAY',
                               text_to_say("ready"),
                               transitions={
                                   'succeeded': 'YesNoTest',
                                   'aborted': 'aborted'
                               })

        smach.StateMachine.add('YesNoTest',
                               SayYesOrNoSM(),
                               transitions={
                                   'succeeded': 'SAY_SUCC',
                                   'aborted': 'SAY_ABORTED'
                               })

        smach.StateMachine.add('SAY_SUCC',
                               text_to_say("SUCCEEDED"),
                               transitions={
                                   'succeeded': 'succeeded',
                                   'aborted': 'aborted'
                               })

        smach.StateMachine.add('SAY_ABORTED',
                               text_to_say("ABORTED"),
                               transitions={
                                   'succeeded': 'succeeded',
                                   'aborted': 'aborted'
                               })

    # This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
    sis = smach_ros.IntrospectionServer('yes_no_introspection', sm,
                                        '/YES_NO_TEST')
    sis.start()

    sm.execute()

    rospy.spin()
    sis.stop()
        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'}) 
Exemple #16
0
    def __init__(self):
        smach.StateMachine.__init__(self,
                                    ['succeeded', 'preempted', 'aborted'],
                                    output_keys=['standard_error'])
        with self:

            # Home position
            smach.StateMachine.add(
                'home_position_init',
                play_motion_sm('home'),
                transitions={
                    'succeeded': 'prepare_Avoid',
                    'aborted':
                    'home_position_init',  #TODO: Change aborted to try again
                    'preempted': 'preempted'
                })

            # 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 location."),
                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 final destination for Avoid That."
                ),
                transitions={'succeeded': 'succeeded'})
def main():
    rospy.init_node('Conc_time_node')

    sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
 
    with sm:
        sm.userdata.sleep_time = 10
        
        STATES = [Sleeper(30)]
        STATE_NAMES = ["sleep"]
        INPUTS = ["sleep_time"]
            
        smach.StateMachine.add('Conc_time',
                            ConcurrenceTime(state_name=STATE_NAMES, states=STATES, inputs=INPUTS, timeout=5),
                            transitions={
                                         'succeeded': 'succeeded', 
                                         'aborted': 'aborted', 'time_ends':'say_time_out'})
        # Say TimeOut 
        smach.StateMachine.add(
                'say_time_out',
                text_to_say("The time is finish"),
                transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 
                'preempted': 'preempted'})
    sm.execute()
 
    rospy.spin()
    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
            self.userdata.type_movment = 'home'
            

            smach.StateMachine.add('INTRO',
                                   acknowledgment(type_movement='home', tts_text=START_FRASE),
                                   transitions={'succeeded': 'DEFAULT_POSITION','aborted':'DEFAULT_POSITION'})
            
            smach.StateMachine.add('DEFAULT_POSITION',
                                   move_head_form("center","up"),
                                   transitions={'succeeded': 'Learn','aborted':'Learn'})

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

            smach.StateMachine.add('SM_STOP_LEARNING',
                                   text_to_say(LETS_GO),
                                   transitions={'succeeded': 'succeeded','aborted':'aborted'})
def main():
    rospy.init_node('Conc_time_node')

    sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])

    with sm:
        sm.userdata.sleep_time = 10

        STATES = [Sleeper(30)]
        STATE_NAMES = ["sleep"]
        INPUTS = ["sleep_time"]

        smach.StateMachine.add('Conc_time',
                               ConcurrenceTime(state_name=STATE_NAMES,
                                               states=STATES,
                                               inputs=INPUTS,
                                               timeout=5),
                               transitions={
                                   'succeeded': 'succeeded',
                                   'aborted': 'aborted',
                                   'time_ends': 'say_time_out'
                               })
        # Say TimeOut
        smach.StateMachine.add('say_time_out',
                               text_to_say("The time is finish"),
                               transitions={
                                   'succeeded': 'succeeded',
                                   'aborted': 'aborted',
                                   'preempted': 'preempted'
                               })
    sm.execute()

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

        with self:
            self.userdata.asr_userSaid = ''    
            self.userdata.grammar_name = GRAMMAR_NAME
            self.userdata.tts_text = None
            self.userdata.tts_wait_before_speaking = None
            self.userdata.tts_lang = None
            
            # Listen 
            smach.StateMachine.add(
                'listen_info',
                ReadASR(),
                transitions={'succeeded': 'process_command', 'aborted': 'aborted', 
                'preempted': 'preempted'}) 
            
            # Prepare the information 
            smach.StateMachine.add(
                "process_command",
                ProcessCommand(),
                transitions={'yes': 'succeeded', 'no': 'aborted', 'aborted':'repeat'})  

            # Ask for repeat
            smach.StateMachine.add(
                'repeat',
                text_to_say("Excuse me, I don't understand you. Can you repeat?"),
                transitions={'succeeded': 'listen_info', 'aborted': 'aborted', 
                'preempted': 'preempted'})             
    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):
        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
            self.userdata.type_movment = 'home'
            
            smach.StateMachine.add('DEFAULT_POSITION',
                                   move_head_form("center","up"),
                                   transitions={'succeeded': 'INTRO','aborted':'INTRO'})
            smach.StateMachine.add('INTRO',
                                   acknowledgment(type_movement='home', tts_text=START_FRASE),
                                   transitions={'succeeded': 'Learn','aborted':'aborted'})

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

            smach.StateMachine.add('SM_STOP_LEARNING',
                                   text_to_say(LETS_GO),
                                   transitions={'succeeded': 'succeeded','aborted':'aborted'})
    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=False), 
                                   transitions={'succeeded':'Move_head_Left', 'aborted':'aborted'})
            
            smach.StateMachine.add('Move_head_Left',
                                   move_head_form( head_left_right='mid_left', head_up_down='up'),
                                   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',
                                   move_head_form( head_left_right='mid_right', head_up_down='up'),
                                   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',
                                   move_head_form( head_left_right='center', head_up_down='up'),
                                   transitions={'succeeded':'Wait_search_1', 'aborted':'Wait_search_1'})
 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'})
    def __init__(self, nav_to_poi_name = None, tts_text = None):
        smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
                                 input_keys=['tts_text','nav_to_poi_name'], 
                                 output_keys=['standard_error'])
        with self:
            self.userdata.tts_lang='en_US'
            self.userdata.tts_wait_before_speaking=0
            
            self.userdata.standard_error='OK'
            
            smach.StateMachine.add(
                                'INIT_VAR',
                                prepare_state(nav_to_poi_name, tts_text),
                                transitions={'succeeded': 'Concurrence_Say_Nav', 'aborted': 'aborted', 
                                'preempted': 'preempted'})    
            
            sm_conc = smach.Concurrence(outcomes=['succeeded', 'preempted','aborted'],
                                        default_outcome='succeeded',
                                        input_keys=['tts_text',
                                                   'nav_to_poi_name',
                                                   'tts_wait_before_speaking',
                                                   'tts_lang'])
            with sm_conc:
                sm_conc.add('Say_conc',
                                text_to_say())

                sm_conc.add('Nav_to_poi_conc',
                                nav_to_poi())
                
            smach.StateMachine.add('Concurrence_Say_Nav', 
                                   sm_conc,
                                   transitions={'succeeded':'succeeded',
                                                 'aborted':'aborted',
                                                 'preempted':'preempted'})
Exemple #27
0
    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'
                                   })
    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'}) 
Exemple #29
0
def main():
    rospy.init_node('say_test')

    sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])

    with sm:

        sm.userdata.tts_text = None
        sm.userdata.tts_wait_before_speaking = None
        sm.userdata.tts_lang = 'en_US'

        smach.StateMachine.add('SaySM',
                               text_to_say("Hello!"),
                               transitions={
                                   'succeeded': 'succeeded',
                                   'aborted': 'aborted'
                               })

    # This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
    sis = smach_ros.IntrospectionServer('saysm_test_introspection', sm,
                                        '/SAY_TEST')
    sis.start()

    sm.execute()

    rospy.spin()
    sis.stop()
Exemple #30
0
    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'),
                                   transitions={
                                       'succeeded': 'Move_head_Left',
                                       'aborted': 'aborted'
                                   })

            smach.StateMachine.add('Move_head_Left',
                                   move_head_form(head_left_right='mid_left',
                                                  head_up_down='normal'),
                                   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',
                                   move_head_form(head_left_right='mid_right',
                                                  head_up_down='normal'),
                                   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',
                                   move_head_form(head_left_right='center',
                                                  head_up_down='normal'),
                                   transitions={
                                       'succeeded': 'succeeded',
                                       'aborted': 'aborted'
                                   })
Exemple #31
0
    def __init__(self, tts=None, poi_name=None):
        """
        Constructor for nav_to_coord.
        """
        #Initialization of the SMACH State machine
        smach.StateMachine.__init__(
            self,
            outcomes=['succeeded', 'preempted', 'aborted'],
            input_keys=[
                'nav_to_coord_goal', 'tts_text', 'tts_wait_before_speaking',
                'tts_lang', 'nav_to_poi_name'
            ],
            output_keys=[])
        self.tts = tts
        self.poi_name = poi_name

        with self:

            smach.StateMachine.add('say',
                                   text_to_say(self.tts, wait=False),
                                   transitions={
                                       'succeeded': 'nav_to_poi',
                                       'aborted': 'aborted'
                                   })

            smach.StateMachine.add('nav_to_poi',
                                   nav_to_poi(self.poi_name),
                                   transitions={
                                       'succeeded': 'succeeded',
                                       'aborted': 'aborted',
                                       'preempted': 'preempted'
                                   })
Exemple #32
0
def main():
    rospy.init_node('say_test')

    sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])

    with sm:

	sm.userdata.tts_text = None
	sm.userdata.tts_wait_before_speaking = None
	sm.userdata.tts_lang = 'en_US'

        smach.StateMachine.add(
            'SaySM',
            text_to_say("Hello!"),
            transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})

    # This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
    sis = smach_ros.IntrospectionServer(
        'saysm_test_introspection', sm, '/SAY_TEST')
    sis.start()

    sm.execute()

    rospy.spin()
    sis.stop()
    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', 'preempted', 'aborted'],
                                    input_keys=[],
                                    output_keys=['emergency_poi_name', 'wave_position', 'wave_yaw_degree'])


        with self:           
            self.userdata.emergency_location = []
            self.userdata.tts_lang = 'en_US'
            self.userdata.tts_wait_before_speaking = 0
            self.userdata.possible_pois = []
            self.userdata.n_item = 0
                        
            smach.StateMachine.add(
                'Prepare_Data',
                Prepare_Data(),
                transitions={'succeeded': 'Search_Emergency'})
                
            smach.StateMachine.add(
                'Search_Emergency',
                Select_Possible_Poi(),
                transitions={'succeeded':'Say_Go_to_Room','finished_searching':'aborted'})
            
            smach.StateMachine.add(
                'Say_Go_to_Room',
                text_to_say("Now I am going to the next room"),
                transitions={'succeeded':'Navigate_to_Room','aborted':'aborted'})
            
            smach.StateMachine.add(
                'Navigate_to_Room',
                nav_to_poi(),
                remapping={'nav_to_poi_name':'nav_to_poi_name_possible'},
                transitions={'succeeded': 'Search_Wave', 'aborted': 'Navigate_to_Room', 'preempted': 'preempted'})
            
            smach.StateMachine.add(
                'Search_Wave',
                Search_Wave_SM(head_position='down',text_for_wave_searching='Where are you? I am trying to find and help you.'),
                transitions={'succeeded':'Normal_head_Out', 'preempted':'preempted', 
                             'aborted':'aborted', 
                             'end_searching':'Normal_head'})
            
            smach.StateMachine.add(
                                   'Normal_head_Out',
                                   move_head_form(head_left_right='center', head_up_down='normal'),
                                   transitions={'succeeded':'Prepare_Output_Search', 'preempted':'Prepare_Output_Search', 
                                                'aborted':'Prepare_Output_Search'})
            smach.StateMachine.add(
                                   'Normal_head',
                                   move_head_form(head_left_right='center', head_up_down='normal'),
                                   transitions={'succeeded':'Search_Emergency', 'preempted':'Search_Emergency', 
                                                'aborted':'Search_Emergency'})
            
            smach.StateMachine.add(
                'Prepare_Output_Search',
                Prepare_output_search(),
                transitions={'succeeded':'succeeded', 
                             'aborted':'aborted'})
Exemple #35
0
    def __init__(self, frame_id='/map'):
        """
        Constructor for nav_to_coord.
        """
        #Initialization of the SMACH State machine
        smach.StateMachine.__init__(
            self,
            outcomes=['succeeded', 'preempted', 'aborted'],
            input_keys=['nav_to_coord_goal', 'nav_to_coord_frame'],
            output_keys=['standard_error'])

        with self:

            smach.StateMachine.add('CreateNavGoal',
                                   createNavGoal(frame_id),
                                   transitions={
                                       'succeeded': 'MoveRobot',
                                       'aborted': 'aborted'
                                   })

            def move_res_cb(userdata, result_status, result):

                if result_status != 3:  # 3 == SUCCEEDED
                    if result_status == 4:
                        userdata.standard_error = "Aborted navigation goal (maybe we didn't get there?)"
                        rospy.loginfo(userdata.standard_error)
                    elif result_status == 5:  # We havent got a rejected yet, maybe never happens
                        userdata.standard_error = "Rejected navigation goal (maybe the goal is outside of the map or in a obstacle?)"
                        rospy.loginfo(userdata.standard_error)
                    elif result_status == 2:
                        return 'preempted'
                    return 'aborted'
                else:
                    userdata.standard_error = "OK"
                    return 'succeeded'

            smach.StateMachine.add('MoveRobot',
                                   SimpleActionState(
                                       NAVIGATION_TOPIC_NAME,
                                       MoveBaseAction,
                                       goal_key='navigation_goal',
                                       input_keys=['standard_error'],
                                       output_keys=['standard_error'],
                                       result_cb=move_res_cb),
                                   transitions={
                                       'succeeded': 'succeeded',
                                       'aborted': 'Aborting_text',
                                       'preempted': 'preempted'
                                   })

            smach.StateMachine.add(
                'Aborting_text',
                text_to_say('I cannot reach the location, sorry.'),
                transitions={
                    'succeeded': 'succeeded',
                    'aborted': 'aborted'
                })
    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, head_position=None, text_for_wave_searching="I am looking for you."):
     smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
                              input_keys=[],
                              output_keys=['wave_position', 'wave_yaw_degree','standard_error'])
     with self:
         self.userdata.loop_iterations = 0
         self.userdata.wave_position = None
         self.userdata.wave_yaw_degree = None
         self.userdata.standard_error = ''
         
         smach.StateMachine('Dummy',
                            DummyStateMachine(),
                            transitions={'succeeded':'Say_follow'})
         smach.StateMachine('Say_follow',
                            text_to_say('Follow Me Please, I will guide you to the person'),
                            transitions={'succeeded':'succeeded', 'aborted':'aborted'})
         
         smach.StateMachine.add(
                                'Move_head_prepare',
                                prepare_move_head(head_position),
                                transitions={'succeeded': 'move_head', 'aborted': 'aborted', 
                                             'preempted': 'preempted'})
         smach.StateMachine.add(
                                'move_head',
                                move_head_form(head_up_down=head_position),
                                transitions={'succeeded': 'Say_Searching',
                                             'preempted':'Say_Searching',
                                             'aborted':'aborted'})
         smach.StateMachine.add(
             'Say_Searching',
             text_to_say("text_for_wave_searching"),
             transitions={'succeeded':'face_detection', 'aborted':'face_detection', 'preempted':'face_detection'})
        
         #Hard-coded maximum time in order to detect wave 
         smach.StateMachine.add(
             'face_detection',
             detect_face(),
             transitions={'succeeded': 'Say_Found', 'aborted': 'aborted', #TODO before it was Move_head_prepare
             'preempted': 'preempted'}) 
         
         smach.StateMachine.add(
             'Say_Found',
             text_to_say("Oh! I have found you finally."),
             transitions={'succeeded':'succeeded', 'aborted':'aborted', 'preempted':'preempted'})
    def __init__(self, object_name = None):
        smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
                    input_keys=['object_name'],
                    output_keys=['standard_error', 'objectd'])

        with self:
        
            smach.StateMachine.add('PrepareData',
               prepareData(object_name),
               transitions={'succeeded':'get_object_info_sm', 'aborted':'aborted'})
            
            # Obtain the location where the object can stay
            smach.StateMachine.add('get_object_info_sm',
                   GetObjectInfoSM(),
                   transitions={'succeeded': 'say_go_to_poi',
                                'aborted': 'aborted',
                                'preempted': 'preempted'})
            
            # say that it goes to the poi
            smach.StateMachine.add(
                'say_go_to_poi',
                text_to_say("I'm going to take the 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_location"},
                transitions={'succeeded': 'say_start_recognition', 'aborted': 'aborted'})
            
            # Say start recognition
            smach.StateMachine.add(
                'say_start_recognition',
                text_to_say("I'm going to start object recognition"),
                transitions={'succeeded': 'object_detection', 'aborted': 'object_detection', 
                'preempted': 'preempted'}) 
                  
            # Object Detection
            smach.StateMachine.add(
                'object_detection',
                recognize_object(),
                transitions={'succeeded': 'succeeded', 'aborted': 'get_object_info_sm'})
Exemple #39
0
    def __init__(self):
        smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted', 'end'],
                                     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'})
                #Cas 1 - Crec que es perdra tota l'estona si esta premut o no
                # transitions= {'succeeded':'check_button_pressed', 'aborted':'aborted', 'preempted':'preempted'})
            
            smach.StateMachine.add(
                'check_button_pressed',
                check_button_pressed(),
                transitions= {'pressed':'say_pressed_button', 'unpressed':'say_not_pressed_button',
                              'succeeded':'sleep_state', 'preempted':'preempted'})
            
            # Sleep
            smach.StateMachine.add(
                'sleep_state',
                Sleeper(2),
                transitions={'succeeded': 'check_button', 'aborted': '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':'end', 'aborted':'end', 'preempted':'preempted'})
    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'
            


            smach.StateMachine.add('INIT_VAR',
                                   init_var(),
                                   transitions={'succeeded': 'SAY_LIFTIME',
                                                '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': 'INIT_VAR'})

            smach.StateMachine.add('READ_TRACKER_TOPIC',
                                   topic_reader(topic_name='/people_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(),
                                   transitions={'succeeded': 'STOP_LEARNING',
                                                'aborted': 'CONTROL_TIME'})

            smach.StateMachine.add('STOP_LEARNING',
                                   text_to_say("OK i have finsished"),
                                   transitions={'succeeded': 'succeeded','aborted':'STOP_LEARNING'})
def main():
    rospy.init_node('yes_no_test')

    sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])

    with sm:
        sm.userdata.grammar_name = None        
        sm.userdata.tts_text = None
        sm.userdata.tts_wait_before_speaking = None
        sm.userdata.tts_lang = None
        
        # Load grammar yes/no
        smach.StateMachine.add(
            'ActivateASR_yesno',
            ActivateASR("robocup/yes_no"),
            transitions={'succeeded': 'DID_YOU_SAY', 'aborted': 'aborted', 'preempted': 'preempted'})                    
        
        smach.StateMachine.add('DID_YOU_SAY',
            text_to_say("ready"),                
            transitions={'succeeded': 'YesNoTest', 'aborted': 'aborted'})

        smach.StateMachine.add('YesNoTest',
            SayYesOrNoSM(),
            transitions={'succeeded': 'SAY_SUCC', 'aborted': 'SAY_ABORTED'})
         
        smach.StateMachine.add('SAY_SUCC',
            text_to_say("SUCCEEDED"),                
            transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})

        smach.StateMachine.add('SAY_ABORTED',
            text_to_say("ABORTED"),                
            transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})
         
    # This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
    sis = smach_ros.IntrospectionServer(
        'yes_no_introspection', sm, '/YES_NO_TEST')
    sis.start()

    sm.execute()

    rospy.spin()
    sis.stop()
Exemple #42
0
    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'
                                   })
Exemple #43
0
    def __init__(self, frame_id="/map"):
        """
        Constructor for nav_to_coord.
        """
        # Initialization of the SMACH State machine
        smach.StateMachine.__init__(
            self,
            outcomes=["succeeded", "preempted", "aborted"],
            input_keys=["nav_to_coord_goal"],
            output_keys=["standard_error"],
        )

        with self:

            smach.StateMachine.add(
                "CreateNavGoal", createNavGoal(frame_id), transitions={"succeeded": "MoveRobot", "aborted": "aborted"}
            )

            def move_res_cb(userdata, result_status, result):
                if result_status != 3:  # 3 == SUCCEEDED
                    rospy.logwarn("Error in NAV_TO_COORD: " + str(result))
                    if result_status == 4:
                        userdata.standard_error = "Aborted navigation goal (maybe we didn't get there?)"
                        rospy.loginfo(userdata.standard_error)
                    elif result_status == 5:  # We havent got a rejected yet, maybe never happens
                        userdata.standard_error = (
                            "Rejected navigation goal (maybe the goal is outside of the map or in a obstacle?)"
                        )
                        rospy.loginfo(userdata.standard_error)
                    elif result_status == 2:
                        return "preempted"
                    return "aborted"
                else:
                    userdata.standard_error = "OK"
                    return "succeeded"

            smach.StateMachine.add(
                "MoveRobot",
                SimpleActionState(
                    NAVIGATION_TOPIC_NAME,
                    MoveBaseAction,
                    goal_key="navigation_goal",
                    input_keys=["standard_error"],
                    output_keys=["standard_error"],
                    result_cb=move_res_cb,
                ),
                transitions={"succeeded": "succeeded", "aborted": "Aborting_text", "preempted": "preempted"},
            )

            smach.StateMachine.add(
                "Aborting_text",
                text_to_say("I cannot reach the location, sorry"),
                transitions={"succeeded": "aborted", "aborted": "aborted"},
            )
Exemple #44
0
    def __init__(self):
        smach.StateMachine.__init__(self,
                                    ['succeeded', 'preempted', 'aborted'])

        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.standard_error = 'OK'
            self.userdata.in_learn_person = 1
            self.userdata.word_to_listen = None

            smach.StateMachine.add('INIT_FOLLOW',
                                   FollowMeInit(),
                                   transitions={
                                       'succeeded': 'FOLLOW_ME_1rst',
                                       'preempted': 'FOLLOW_ME_1rst',
                                       'aborted': 'FOLLOW_ME_1rst'
                                   })

            smach.StateMachine.add('FOLLOW_ME_1rst',
                                   follow_me_1st(),
                                   transitions={
                                       'succeeded': 'TEXT',
                                       'aborted': 'aborted',
                                       'preempted': 'TEXT',
                                       'operator_say_out': 'FOLLOW_ME_3rd'
                                   })

            # in this state i will wait that the door it comes open
            smach.StateMachine.add('TEXT',
                                   text_to_say(LIFT_TEXT),
                                   transitions={
                                       'succeeded': 'FOLLOW_ME_2nd',
                                       'aborted': 'aborted'
                                   })

            # im looking for go out of the lift
            smach.StateMachine.add('FOLLOW_ME_2nd',
                                   follow_me_2nd(),
                                   transitions={
                                       'succeeded': 'FOLLOW_ME_3rd',
                                       'aborted': 'aborted'
                                   })

            smach.StateMachine.add('FOLLOW_ME_3rd',
                                   follow_me_3rd(),
                                   transitions={
                                       'succeeded': 'succeeded',
                                       'preempted': 'preempted',
                                       'aborted': 'aborted'
                                   })
Exemple #45
0
    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'
                                   })
Exemple #46
0
    def __init__(self):
        smach.StateMachine.__init__(
            self,
            outcomes=['succeeded', 'preempted', 'aborted'],
            output_keys=['standard_error'])

        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

            smach.StateMachine.add('INIT_VAR',
                                   init_var(),
                                   transitions={
                                       'succeeded': 'CONCURRENCE',
                                       'aborted': 'aborted',
                                       'preempted': 'preempted'
                                   })

            sm = smach.Concurrence(
                outcomes=['DOOR_OPEN', 'OPERATOR', 'aborted'],
                default_outcome='DOOR_OPEN',
                input_keys=['word_to_listen'],
                child_termination_cb=child_term_cb,
                outcome_cb=out_cb)

            with sm:

                # it will finisheed with succeeded if it check a door

                sm.add('CHECK_DOOR', look_for_elevator_door())
                # here i have to listen if they say me to get out of the lift
                sm.add('LISTEN_OPERATOR_FOR_EXIT',
                       ListenWordSM_Concurrent("go out"))

            smach.StateMachine.add('CONCURRENCE',
                                   sm,
                                   transitions={
                                       'OPERATOR': 'SAY_OUT',
                                       'DOOR_OPEN': 'SAY_OUT',
                                       'aborted': 'CONCURRENCE'
                                   })

            # it says i'm going out
            smach.StateMachine.add('SAY_OUT',
                                   text_to_say(SAY_OUT_FRASE),
                                   transitions={
                                       'succeeded': 'succeeded',
                                       'aborted': 'aborted',
                                       'preempted': 'preempted'
                                   })
 def __init__(self):
     smach.StateMachine.__init__(self,
                                 outcomes=['succeeded', 'aborted', 'preempted'],
                                 input_keys=['tts_text'],
                                 output_keys=[])
     with self:
         smach.StateMachine.add('random_prepare',
                                random_speech_prepare(),
                                transitions={'succeeded':'say_random'})
         smach.StateMachine.add('say_random',
                                text_to_say(),
                                transitions={'succeeded':'succeeded','aborted':'aborted'})
    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.head_left_right=None
            self.userdata.head_up_down=None
            self.userdata.standard_error='OK'
            self.userdata.in_learn_person=1

            
            smach.StateMachine.add('DEFAULT_POSITION',
                                   move_head_form("center","up"),
                                   transitions={'succeeded': 'INTRO','aborted':'INTRO'})

            smach.StateMachine.add('INTRO',
                                   text_to_say(START_FRASE),
                                   transitions={'succeeded': 'START_FOLLOWING_COME_CLOSER','aborted':'START_FOLLOWING_COME_CLOSER'})
            
            smach.StateMachine.add('Listen',
                                   ListenWordSM_Concurrent("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(learn_face=True),
                                   transitions={'succeeded': 'SM_STOP_LEARNING',
                                                'aborted': 'aborted'})

            smach.StateMachine.add('SM_STOP_LEARNING',
                                   text_to_say(LEARNED_PERSON_FRASE),
                                   transitions={'succeeded': 'succeeded','aborted':'aborted'})
Exemple #49
0
    def __init__(self, type_movement="yes", tts_text=''):
        smach.StateMachine.__init__(
            self,
            outcomes=['succeeded', 'aborted', 'preempted'],
            input_keys=['tts_text', 'type_movment'],
            output_keys=['standard_error'])

        self.type_movement = type_movement
        self.tts_text = tts_text

        with self:
            self.userdata.tts_text = None
            self.userdata.type_movment = None
            self.userdata.tts_lang = None
            self.userdata.tts_wait_before_speaking = 0
            self.userdata.standard_error = 'OK'
            self.userdata.manip_time_to_play = 30
            self.userdata.skip_planning = False

            smach.StateMachine.add('INIT_VAR',
                                   init_var(self.type_movement, self.tts_text),
                                   transitions={
                                       'succeeded': 'PUT_MOVMENT',
                                       'aborted': 'aborted',
                                       'preempted': 'preempted'
                                   })

            sm = smach.Concurrence(
                outcomes=['succeeded', 'preempted', 'aborted'],
                default_outcome='succeeded',
                input_keys=[
                    'tts_text', 'manip_motion_to_play', 'manip_time_to_play',
                    'tts_wait_before_speaking', 'tts_lang', 'skip_planning'
                ])
            self.userdata.tts_wait_before_speaking = 0
            self.userdata.tts_text = None
            self.userdata.tts_lang = None

            with sm:

                sm.add('SAY', text_to_say())

                sm.add('MOVE', play_motion_sm(skip_planning=False))

            smach.StateMachine.add('PUT_MOVMENT',
                                   sm,
                                   transitions={
                                       'succeeded': 'succeeded',
                                       'aborted': 'aborted',
                                       'preempted': 'preempted'
                                   })
Exemple #50
0
    def __init__(self, grammar=None):
        smach.StateMachine.__init__(
            self,
            outcomes=['succeeded', 'preempted', 'aborted'],
            input_keys=[],
            output_keys=[])

        with self:
            self.userdata.asr_userSaid = ''
            self.userdata.grammar_name = GRAMMAR_NAME
            self.userdata.tts_text = None
            self.userdata.tts_wait_before_speaking = None
            self.userdata.tts_lang = None

            # Activate the server
            smach.StateMachine.add('ActivateASR',
                                   ActivateASR(GRAMMAR_NAME),
                                   transitions={
                                       'succeeded': 'listen_info',
                                       'aborted': 'aborted',
                                       'preempted': 'preempted'
                                   })

            # Listen
            smach.StateMachine.add('listen_info',
                                   ReadASR(),
                                   transitions={
                                       'succeeded': 'process_command',
                                       'aborted': 'aborted',
                                       'preempted': 'preempted'
                                   })

            # Prepare the information
            smach.StateMachine.add("process_command",
                                   ProcessCommand(),
                                   transitions={
                                       'yes': 'succeeded',
                                       'no': 'aborted',
                                       'aborted': 'aborted'
                                   })

            # Ask for repeat
            smach.StateMachine.add(
                'repeat',
                text_to_say(
                    "Excuse me, I don't understand you. Can you repeat?"),
                transitions={
                    'succeeded': 'aborted',
                    'aborted': 'aborted',
                    'preempted': 'preempted'
                })
Exemple #51
0
    def __init__(self):
        smach.StateMachine.__init__(self,
                                    ['succeeded', 'preempted', 'aborted'],
                                    output_keys=[],
                                    input_keys=[])

        with self:

            #maybe i will have to learn again
            smach.StateMachine.add('INIT_VAR',
                                   init_var(),
                                   transitions={
                                       'succeeded': 'look_wave',
                                       'aborted': 'aborted',
                                       'preempted': 'preempted'
                                   })

            #maybe i will have to learn again
            smach.StateMachine.add('look_wave',
                                   WaveDetection(),
                                   transitions={
                                       'succeeded': 'say_hello',
                                       'aborted': 'look_wave',
                                       'preempted': 'preempted'
                                   })

            smach.StateMachine.add('say_hello',
                                   text_to_say(Say, wait=False),
                                   transitions={
                                       'succeeded': 'wait_state',
                                       'aborted': 'wait_state'
                                   })

            smach.StateMachine.add('play_motion_state',
                                   play_motion_sm('wave', skip_planning=True),
                                   transitions={
                                       'succeeded': 'wait_state',
                                       'preempted': 'wait_state',
                                       'aborted': 'wait_state'
                                   })

            smach.StateMachine.add('wait_state',
                                   waitstate(),
                                   transitions={
                                       'succeeded': 'look_wave',
                                       'aborted': 'look_wave',
                                       'preempted': 'look_wave'
                                   })
Exemple #52
0
 def __init__(self):
     smach.StateMachine.__init__(
         self,
         outcomes=['succeeded', 'aborted', 'preempted'],
         input_keys=['tts_text'],
         output_keys=[])
     with self:
         smach.StateMachine.add('random_prepare',
                                random_speech_prepare(),
                                transitions={'succeeded': 'say_random'})
         smach.StateMachine.add('say_random',
                                text_to_say(),
                                transitions={
                                    'succeeded': 'succeeded',
                                    'aborted': 'aborted'
                                })
Exemple #53
0
def main():
    rospy.loginfo('follow_operator_TEST')
    rospy.init_node('follow_operator_TEST')

    sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
    with sm:
        sm.userdata.standard_error = 'OK'
        # it prepare the name and the function for drope
        smach.StateMachine.add('prepare_msg',
                               prepare_msg(),
                               transitions={
                                   'succeeded': 'follow_operator',
                                   'aborted': 'aborted',
                                   'preempted': 'preempted'
                               })
        # it call the drop_face state
        smach.StateMachine.add('follow_operator',
                               FollowOperator(),
                               transitions={
                                   'lost': 'say_lost',
                                   'succeeded': 'follow_me_info'
                               })

        sm.userdata.tts_text = "i have lose the person"
        sm.userdata.tts_wait_before_speaking = 0
        smach.StateMachine.add('say_lost',
                               text_to_say(),
                               transitions={
                                   'succeeded': 'follow_me_info',
                                   'aborted': 'follow_me_info'
                               })

        smach.StateMachine.add('follow_me_info',
                               follow_operator_error(),
                               transitions={
                                   'succeeded': 'succeeded',
                                   'aborted': 'aborted'
                               })

    # This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
    sis = smach_ros.IntrospectionServer('follow_operator_test', sm, '/FO_ROOT')
    sis.start()

    sm.execute()

    rospy.spin()
    sis.stop()
Exemple #54
0
    def __init__(self):
        smach.StateMachine.__init__(
            self,
            outcomes=['succeeded', 'preempted', 'aborted'],
            input_keys=['name', 'nav_to_poi_name', 'face', 'face_frame'],
            output_keys=['face', 'standard_error', 'face_frame'])

        with self:

            self.userdata.num_iterations = 0
            self.userdata.face = None
            self.userdata.wait_time = 5

            smach.StateMachine.add(
                'Say_Searching',
                text_to_say('Right Now I am looking for the Ambulance.'),
                transitions={
                    'succeeded': 'Concurrence',
                    'aborted': 'aborted',
                    'preempted': 'preempted'
                })

            # Concurrence
            sm_conc = smach.Concurrence(
                outcomes=['succeeded', 'aborted', 'preempted'],
                default_outcome='succeeded',
                input_keys=['name', 'nav_to_poi_name', 'face', 'wait_time'],
                output_keys=['face', 'standard_error', 'face_frame'],
                child_termination_cb=child_term_cb,
                outcome_cb=out_cb)

            with sm_conc:
                # Search for face
                smach.Concurrence.add('find_faces',
                                      recognize_face_concurrent())

                smach.Concurrence.add('say_search_faces',
                                      say_searching_faces())

            smach.StateMachine.add('Concurrence',
                                   sm_conc,
                                   transitions={
                                       'succeeded': 'succeeded',
                                       'aborted': 'aborted',
                                       'preempted': 'aborted'
                                   })
Exemple #55
0
    def __init__(self):
        smach.StateMachine.__init__(self,
                                    ['succeeded', 'preempted', 'aborted'])
        #rospy.init_node("SM_GPSR_ORDERS")
        with self:

            smach.StateMachine.add(
                'Stand_Still',
                text_to_say(text="Please don't move for a moment",
                            wait_before_speaking=0),
                transitions={'succeeded': 'DataBase_and_Learn'})

            smach.StateMachine.add('DataBase_and_Learn',
                                   new_database_and_learn(
                                       name_face='referee',
                                       name_database='Soar'),
                                   transitions={'succeeded': 'succeeded'})
Exemple #56
0
    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'
                                   })
Exemple #57
0
    def __init__(self):
        smach.StateMachine.__init__(self,
                                    ['succeeded', 'preempted', 'aborted'],
                                    input_keys=['person_location'])

        with self:
            self.userdata.emergency_location = []
            self.userdata.tts_lang = 'en_US'
            self.userdata.tts_wait_before_speaking = 0

            #It should be Speech Recognition: ListenTo(?)
            smach.StateMachine.add('Prepare_Ask_Status',
                                   prepare_tts('Are you Ok?'),
                                   transitions={
                                       'succeeded': 'Ask_Status',
                                       'aborted': 'Ask_Status',
                                       'preempted': 'Ask_Status'
                                   })
            smach.StateMachine.add('Ask_Status',
                                   text_to_say(),
                                   transitions={
                                       'succeeded': 'Save_Info',
                                       'aborted': 'Save_Info',
                                       'preempted': 'Save_Info'
                                   })
            #TODO: Do we have to add an SayYesOrNoSM?
            # smach.StateMachine.add(
            #     'Yes_No',
            #     SayYesOrNoSM(),
            #     transitions={'succeeded':'Register_Position', 'aborted':'Register_Position', 'preempted':'Register_Position'})

            #TODO: Actually Generate the PDF (TODO: Look for USB/Path...)
            #Save_Info(): Saves the emergency info and generates a pdf file
            #input_keys: emergency_location
            smach.StateMachine.add(
                'Save_Info',
                DummyStateMachine(),
                #GeneratePDF_State(),
                transitions={
                    'succeeded': 'succeeded',
                    'aborted': 'aborted',
                    'preempted': 'preempted'
                })
Exemple #58
0
def main():
    rospy.init_node('cocktail_party')

    sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])

    with sm:
        STATES = [CocktailPartySM()]
        STATE_NAMES = ["CocktailPartySM"]
    
        # We have 10 minuts -> 600 sec
#         smach.StateMachine.add(
#             "Cocktail_test",
#             ConcurrenceTime(states=STATES, state_names=STATE_NAMES, timeout=570),
#             transitions={'succeeded': 'leaving_arena', 'aborted': "Say_timeout"})
        smach.StateMachine.add(
            "Cocktail_test",
            CocktailPartySM(),
            transitions={'succeeded': 'leaving_arena', 'aborted': "Say_timeout"})
            
            
        # Say TimeOut
        smach.StateMachine.add(
            'Say_timeout',
            text_to_say("My time to do the test is over. I going to leave the arena", wait=False),
            transitions={'succeeded': 'leaving_arena', 'aborted': 'leaving_arena'})

        # Leaving the arena  
        smach.StateMachine.add(
            'leaving_arena',
            nav_to_poi('leave_arena'),
            transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'}) 
    
    # This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
    sis = smach_ros.IntrospectionServer(
        'cocktail_party', sm, '/CP_ROOT')
    sis.start()

    sm.execute()

    rospy.spin()
    sis.stop()
Exemple #59
0
    def __init__(self, GRAMMAR_NAME):
        smach.StateMachine.__init__(
            self,
            outcomes=['succeeded', 'preempted', 'aborted'],
            input_keys=['delete_database'],
            output_keys=['name_face', 'object_name'])
        self.grammar_name = GRAMMAR_NAME
        with self:

            # Ask for order
            smach.StateMachine.add('ask_order',
                                   AskQuestionSM(
                                       "what would you want to tell me",
                                       self.grammar_name,
                                       calibrate=True,
                                       Bucle=False,
                                       time_calibrate=10),
                                   transitions={
                                       'succeeded': 'succeeded',
                                       'aborted': 'ask_order',
                                       'preempted': 'preempted'
                                   })

            # Process the answer
            smach.StateMachine.add(
                'process_order',
                process_order(),
                transitions={
                    'succeeded': 'say_got_it',
                    'aborted': 'ask_order',  # TODO before it was 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'
                                   })