def main():
    rospy.init_node('recognize_obect_test')
    
    sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
    with sm:
        # smach.StateMachine.add(
        #     'prepare_object',
        #     prepare_detect_object(),
        #     transitions={'succeeded': 'detect_object','aborted' : 'aborted_info'})
        # it call the learn state
        smach.StateMachine.add(
            'detect_object',
            object_detect_sm('Barritas'),
            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(
        'detect_object_introspection', sm, '/DFI_ROOT')
    sis.start()

    sm.execute()

    rospy.spin()
    sis.stop()
Пример #2
0
def main():
    rospy.init_node('recognize_obect_test')

    sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
    with sm:
        # smach.StateMachine.add(
        #     'prepare_object',
        #     prepare_detect_object(),
        #     transitions={'succeeded': 'detect_object','aborted' : 'aborted_info'})
        # it call the learn state
        smach.StateMachine.add('detect_object',
                               object_detect_sm('pringles'),
                               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('detect_object_introspection', sm,
                                        '/DFI_ROOT')
    sis.start()

    sm.execute()

    rospy.spin()
    sis.stop()
Пример #3
0
 def __init__(self):
     smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
                              input_keys=['object_name'], 
                              output_keys=['standard_error','object_position','object_detected_name'])
     
     with self:
         self.userdata.standard_error = ''
         smach.StateMachine.add(
                                'Look_down',
                                move_head_form(head_left_right="center", head_up_down="down"),
                                transitions={'succeeded': 'sleep_state', 
                                             'aborted': 'aborted', 
                                             'preempted': 'preempted'})
         # Only for rh2c 
         smach.StateMachine.add(
                 'sleep_state',
                 Sleeper(5),
                 transitions={'succeeded': 'detect_object', 'aborted': 'aborted', 
                 'preempted': 'preempted'})
         
         smach.StateMachine.add(
                 'detect_object',
                 object_detect_sm(),
                 transitions={'succeeded': 'process_object', 'aborted': 'aborted', 
                 'preempted': 'preempted'})
         
         smach.StateMachine.add(
                 'process_object',
                 process_object(),
                 transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 
                 'preempted': 'preempted'})
Пример #4
0
def call_find_object(object_name,world): #TODO 
    
    tosay = "I'm going to search for " + object_name
    speak = speaker(tosay)
    speak.execute()
    rospy.logwarn('call_find_object '+object_name)  
    #############################################################################
    if SKILLS :      
        if (time.time()-TIME_INIT) > 270:
            return "succeeded"
        
        out = 'aborted'
        tries = 0
    
        current_position = world.get_current_position()        
        if current_position in ROOMS:
            room = rospy.get_param('/robocup_params/room/' + current_position)
                
        #while(out=='aborted' and tries<3):      
            for table in room :
                if out == 'succeeded' and tries == 3:
                    break
                call_go_to(table)        
                 
                sm = object_detect_sm()#
                out = sm.execute()      #          #PROVABLY WE WILL HAVE TO CHECK IF THERE IS A TABLE NEARBY BEFORE STARTING TO SEARCH
                object_position = sm.userdata.object_pose #
                tries = tries+1#
            
            
        if out=='aborted':
            tosay = "I couldn't find the " + object_name + " you asked for. It isn't here. I'm going to the referee to inform"
            speak = speaker(tosay)
            speak.execute()
            rospy.logwarn('FAIL IN FINDING ' + object_name)
            time.sleep(SLEEP_TIME)
            
            call_go_to('referee',world)
            tosay = "I couldn't find the " + object_name + " you asked for. It isn't there. I'm afraid that sentence was from category 3"
            speak = speaker(tosay)
            speak.execute() 
            
            return "aborted"
    #############################################################################
    time.sleep(SLEEP_TIME)
    return "succeeded"