def main():
    rospy.init_node('ask_name_test')

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

    with sm:

        sm.userdata.name = "robocup"
        sm.userdata.purgeAll = True

        smach.StateMachine.add(
            'dropfaces',
            drop_faces(),
            transitions={'succeeded': 'SaveFaceSM', 'aborted': 'aborted'})


        smach.StateMachine.add(
            'SaveFaceSM',
            SaveFaceSM(),
            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(
        'saveface_test_introspection', sm, '/SAVEFACE_TEST')
    sis.start()

    sm.execute()

    rospy.spin()
    sis.stop()
def main():
    rospy.init_node('ask_name_test')

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

    with sm:

        sm.userdata.name = "robocup"
        sm.userdata.purgeAll = True

        smach.StateMachine.add('dropfaces',
                               drop_faces(),
                               transitions={
                                   'succeeded': 'SaveFaceSM',
                                   'aborted': 'aborted'
                               })

        smach.StateMachine.add('SaveFaceSM',
                               SaveFaceSM(),
                               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('saveface_test_introspection', sm,
                                        '/SAVEFACE_TEST')
    sis.start()

    sm.execute()

    rospy.spin()
    sis.stop()
def main():
    rospy.init_node('drop_face_test')

    sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
    with sm:
        # it prepare the name and the function for drope
        smach.StateMachine.add('prepare_learn_face',
                               prepare_drop_faces(),
                               transitions={
                                   'succeeded': 'learn_face',
                                   'aborted': 'aborted',
                                   'preempted': 'preempted'
                               })
        # it call the drop_face state
        smach.StateMachine.add('learn_face',
                               drop_faces(),
                               transitions={
                                   'succeeded': 'dummy',
                                   'aborted': 'aborted_info'
                               })
        # it prints the standard error
        smach.StateMachine.add('aborted_info',
                               drop_faces_error(),
                               transitions={
                                   'succeeded': 'dummy',
                                   'aborted': 'dummy'
                               })
        #dummy_state
        smach.StateMachine.add('dummy',
                               dummy_state(),
                               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('drop_faces_introspection', sm,
                                        '/DFI_ROOT')
    sis.start()

    sm.execute()

    rospy.spin()
    sis.stop()
def main():
    rospy.init_node('drop_face_test')

    sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
    with sm:
        # it prepare the name and the function for drope
        smach.StateMachine.add(
            'prepare_learn_face',
            prepare_drop_faces(),
            transitions={'succeeded':'learn_face','aborted' : 'aborted','preempted':'preempted'})
        # it call the drop_face state
        smach.StateMachine.add(
            'learn_face',
            drop_faces(),
            transitions={'succeeded': 'dummy','aborted' : 'aborted_info'})
        # it prints the standard error
        smach.StateMachine.add(
            'aborted_info',
            drop_faces_error(),
            transitions={'succeeded': 'dummy', 'aborted':'dummy'})
        #dummy_state
        smach.StateMachine.add(
            'dummy',
            dummy_state(),
            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(
        'drop_faces_introspection', sm, '/DFI_ROOT')
    sis.start()

    sm.execute()

    rospy.spin()
    sis.stop()
Exemple #5
0
    def __init__(self):
        smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])

        with self:
            # We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
            self.userdata.nav_to_poi_name=''
            
            # Params for drop faces - always will be the same
            self.userdata.name_database = "find_me_database"
            self.userdata.purgeAll = True
            
            # Init the state machine - Drop faces in the database
            smach.StateMachine.add(
                'init_database',
                drop_faces(),
                remapping={'name':'name_database'},
                transitions={'succeeded': 'save_face', 'aborted': 'aborted', 
                'preempted': 'preempted'}) 
            
            # Save the data from the TC
            # We need one state machine that listen the name and then learn the face
            smach.StateMachine.add(
                'save_face',
                SaveFaceSM(),
                transitions={'succeeded': 'prepare_location', 'aborted': 'aborted', 
                'preempted': 'preempted'}) 

            # Prepare the info to go to the room
            smach.StateMachine.add(
                'prepare_location',
                prepare_location(),
                transitions={'succeeded': 'go_location', 'aborted': 'aborted', 
                'preempted': 'preempted'})  

            # Go to the location
            smach.StateMachine.add(
                'go_location',
                nav_to_poi(),
                transitions={'succeeded': 'search_faces', 'aborted': 'aborted', 
                'preempted': 'preempted'})    
           
            # Go around the room, it the robot find the TC return success
            smach.StateMachine.add(
                'search_faces',
                SearchFacesSM(),
                transitions={'succeeded': 'prepare_coord_person', 'aborted': 'aborted', 
                'preempted': 'preempted'}) 

            # Go to the person - We assume that find person will return the position for the person
            smach.StateMachine.add(
                'prepare_coord_person',
                prepare_coord_person(),
                transitions={'succeeded': 'go_to_person', 'aborted': 'aborted', 
                'preempted': 'preempted'})                    
            
            smach.StateMachine.add(
                'go_to_person',
                nav_to_coord(),
                transitions={'succeeded': 'prepare_say_found', 'aborted': 'aborted', 
                'preempted': 'preempted'})                    

            # Say "I found you!"
            smach.StateMachine.add(
                'prepare_say_found',
                prepare_say_found(),
                transitions={'succeeded': 'say_found', 'aborted': 'aborted', 'preempted': 'preempted'})

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

            # Recognize the direction
            smach.StateMachine.add(
                'gesture_recognition',
                DummyStateMachine(),
                transitions={'succeeded': 'go_side', 'aborted': 'aborted', 
                'preempted': 'preempted'})    

            # Go to the direction
            smach.StateMachine.add(
                'go_side',
                DummyStateMachine(),
                transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 
                'preempted': 'preempted'})