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'}) 
Esempio n. 2
0
    def __init__(self, object_name=None):
        smach.StateMachine.__init__(
            self,
            outcomes=['succeeded', 'preempted', 'aborted'],
            input_keys=['object_name'],
            output_keys=[
                'standard_error', 'object_position', 'object_detected_name'
            ])

        with self:
            self.userdata.number_search_object = 0
            #self.userdata.number_search_object_POI = 0
            self.userdata.nav_first_pass = True

            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 indicated in the previous SM
            smach.StateMachine.add('say_go_to_poi',
                                   text_to_say("I'm going to take the object",
                                               wait=False),
                                   transitions={
                                       'succeeded': 'go_to_object',
                                       'aborted': 'aborted',
                                       'preempted': 'preempted'
                                   })

            # Go to poi where the object stays
            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",
                                       wait=False),
                                   transitions={
                                       'succeeded': 'object_detection',
                                       'aborted': 'object_detection',
                                       'preempted': 'preempted'
                                   })

            # Object Detection
            smach.StateMachine.add(
                'object_detection',
                recognize_object(),
                #transitions={'succeeded': 'analyze_object_data', 'aborted': 'get_object_info_sm'})
                transitions={
                    'succeeded': 'analyze_object_data',
                    'aborted': 'fail_object_detection'
                })

            smach.StateMachine.add('analyze_object_data',
                                   analyze_object_data(),
                                   transitions={
                                       'succeeded': 'say_found_object',
                                       'aborted': 'object_detection'
                                   })

            # Say found the object
            #TODO: Tell x, y, z of the object
            smach.StateMachine.add('say_found_object',
                                   text_to_say("I found the object"),
                                   transitions={
                                       'succeeded': 'succeeded',
                                       'aborted': 'succeeded',
                                       'preempted': 'succeeded'
                                   })

            smach.StateMachine.add('fail_object_detection',
                                   fail_object_detection_check(),
                                   transitions={
                                       'aborted': 'pick_anything_prepare',
                                       'succeeded': 'say_re_detect'
                                   })
            smach.StateMachine.add(
                'say_re_detect',
                text_to_say(
                    'I am trying again to detect the object you asked'),
                transitions={
                    'succeeded': 'object_detection',
                    'aborted': 'aborted'
                })

            smach.StateMachine.add('fail_object_detection_poi',
                                   fail_object_detection_check_poi(),
                                   transitions={
                                       'succeeded': 'say_re_go',
                                       'aborted': 'aborted'
                                   })
            smach.StateMachine.add(
                'say_re_go',
                text_to_say(
                    "I am going to another place to see if the object I am looking is there"
                ),
                transitions={
                    'succeeded': 'Prepare_next_poi',
                    'aborted': 'aborted'
                })
            smach.StateMachine.add('Prepare_next_poi',
                                   prepare_next_poi(),
                                   transitions={
                                       'succeeded': 'go_to_object',
                                       'aborted': 'pick_anything_prepare'
                                   })
            smach.StateMachine.add('pick_anything_prepare',
                                   pick_anything_prepare(),
                                   transitions={
                                       'succeeded': 'succeeded',
                                       'aborted': 'aborted'
                                   })
Esempio n. 3
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
        world.item.object_pose = PoseWithCovarianceStamped()

        current_position = world.get_current_position()
        rospy.logwarn(current_position)
        rospy.logwarn(ROOMS)
        rospy.loginfo('-----------------------------------------')
        if current_position in ROOMS:
            rospy.loginfo('-----------------------------------------')
            room = rospy.get_param('/robocup_params/room/' + current_position)
            for table in room:
                rospy.loginfo('-----------------------------------------')
                if world.item.object_pose.header.frame_id != '':
                    rospy.logwarn(
                        "NO ESTEM BUSCAN PERK YA HEM TROBAT EL QUE VOLIEM")
                    break
                call_go_to(table.replace(" ", "_"), world)
                tries = 0

                while (world.item.object_pose.header.frame_id == ''
                       and tries < 3):
                    sm = recognize_object()
                    sm.userdata.object_name = object_name[0].upper(
                    ) + object_name[1:]
                    out = sm.execute()  #
                    names = sm.userdata.object_detected_name
                    poses = sm.userdata.object_position
                    rospy.loginfo('-----------------------------------------')
                    rospy.loginfo(names)
                    rospy.loginfo('-----------------------------------------')
                    rospy.loginfo(poses)
                    rospy.loginfo('-----------------------------------------')
                    tries = tries + 1

                    i = 0
                    while i < len(names):
                        if names[i] == (object_name[0].upper() +
                                        object_name[1:]):
                            world.item.object_pose = poses[i]
                        i = i + 1

        if current_position.replace("_", " ") in TABLES:
            while (world.item.object_pose.header.frame_id == '' and tries < 3):
                sm = recognize_object()
                sm.userdata.object_name = object_name[0].upper(
                ) + object_name[1:]
                out = sm.execute()
                names = sm.userdata.object_detected_name
                poses = sm.userdata.object_position
                rospy.loginfo('-----------------------------------------')
                rospy.loginfo(names)
                rospy.loginfo('-----------------------------------------')
                rospy.loginfo(poses)
                rospy.loginfo('-----------------------------------------')
                tries = tries + 1

                i = 0
                while i < len(names):
                    if names[i] == (object_name[0].upper() + object_name[1:]):
                        world.item.object_pose = poses[i]
                    i = i + 1

        rospy.logwarn(world.item.object_pose)

        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. This sentence is from category 3"
            speak = speaker(tosay)
            speak.execute()

            return "aborted"
    #############################################################################
    time.sleep(SLEEP_TIME)
    return "succeeded"
Esempio n. 4
0
    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'
                                   })
Esempio n. 5
0
    def __init__(self, object_name = None):
        smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
                    input_keys=['object_name'],
                    output_keys=['standard_error', 'object_position', 'object_detected_name'])

        with self:
            self.userdata.number_search_object = 0
            #self.userdata.number_search_object_POI = 0
            self.userdata.nav_first_pass = True
            
            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 indicated in the previous SM
            smach.StateMachine.add( 
                'say_go_to_poi',
                text_to_say("I'm going to take the object", wait=False),
                transitions={'succeeded': 'go_to_object', 'aborted': 'aborted', 
                'preempted': 'preempted'}) 
            
            # Go to poi where the object stays
            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",wait=False),
                transitions={'succeeded': 'object_detection', 'aborted': 'object_detection', 
                'preempted': 'preempted'}) 
                  
            # Object Detection
            smach.StateMachine.add(
                'object_detection',
                recognize_object(),
                #transitions={'succeeded': 'analyze_object_data', 'aborted': 'get_object_info_sm'})
                transitions={'succeeded':'analyze_object_data', 'aborted':'fail_object_detection'})
            
            smach.StateMachine.add(
                'analyze_object_data',
                analyze_object_data(),
                transitions={'succeeded': 'say_found_object', 'aborted': 'object_detection'})
            
            # Say found the object
            #TODO: Tell x, y, z of the object
            smach.StateMachine.add(
                'say_found_object',
                text_to_say("I found the object"),
                transitions={'succeeded': 'succeeded', 'aborted': 'succeeded', 
                'preempted': 'succeeded'}) 
            
            smach.StateMachine.add(
                'fail_object_detection',
                fail_object_detection_check(),
                transitions={'aborted':'fail_object_detection_poi',
                             'succeeded':'say_re_detect'})
            smach.StateMachine.add(
                'say_re_detect',
                text_to_say('I am trying again to detect the object you asked'),
                transitions={'succeeded':'object_detection', 'aborted':'aborted'})
            
            
            smach.StateMachine.add(
                'fail_object_detection_poi',
                fail_object_detection_check_poi(),
                transitions={'succeeded':'say_re_go', 'aborted':'aborted'})
            smach.StateMachine.add(
                'say_re_go',
                text_to_say("I am going to another place to see if the object I am looking is there"),
                transitions={'succeeded':'Prepare_next_poi', 'aborted':'aborted'})
            smach.StateMachine.add(
                'Prepare_next_poi',
                prepare_next_poi(),
                transitions={'succeeded':'go_to_object', 'aborted':'pick_anything_prepare'})
            smach.StateMachine.add(
                'pick_anything_prepare',
                pick_anything_prepare(),
                transitions={'succeeded':'succeeded', 'aborted':'aborted'})
Esempio n. 6
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
        world.item.object_pose = PoseWithCovarianceStamped()
        
        current_position = world.get_current_position()    
        rospy.logwarn(current_position)
        rospy.logwarn(ROOMS)
        rospy.loginfo('-----------------------------------------')
        if current_position in ROOMS:
            rospy.loginfo('-----------------------------------------')
            room = rospy.get_param('/robocup_params/room/' + current_position)
            for table in room :
                rospy.loginfo('-----------------------------------------')
                if world.item.object_pose.header.frame_id!='':
                    rospy.logwarn("NO ESTEM BUSCAN PERK YA HEM TROBAT EL QUE VOLIEM")
                    break
                call_go_to(table.replace(" ","_"),world)        
                tries = 0
                
                while(world.item.object_pose.header.frame_id=='' and tries<3):   
                    sm = recognize_object()
                    sm.userdata.object_name = object_name[0].upper() + object_name[1:]                
                    out = sm.execute()#
                    names = sm.userdata.object_detected_name
                    poses = sm.userdata.object_position
                    rospy.loginfo('-----------------------------------------')
                    rospy.loginfo(names)
                    rospy.loginfo('-----------------------------------------')
                    rospy.loginfo(poses)
                    rospy.loginfo('-----------------------------------------')
                    tries = tries+1
                
                    i = 0
                    while i<len(names):                    
                        if names[i] == (object_name[0].upper() + object_name[1:]):
                            world.item.object_pose = poses[i]
                        i = i +1                        
        
        if current_position.replace("_"," ") in TABLES:       
            while(world.item.object_pose.header.frame_id=='' and tries<3):   
                sm = recognize_object()
                sm.userdata.object_name = object_name[0].upper() + object_name[1:]                
                out = sm.execute()
                names = sm.userdata.object_detected_name
                poses = sm.userdata.object_position
                rospy.loginfo('-----------------------------------------')
                rospy.loginfo(names)
                rospy.loginfo('-----------------------------------------')
                rospy.loginfo(poses)
                rospy.loginfo('-----------------------------------------')
                tries = tries+1
            
                i = 0
                while i<len(names):                    
                    if names[i] == (object_name[0].upper() + object_name[1:]):
                        world.item.object_pose = poses[i]
                    i = i +1
                             
        rospy.logwarn(world.item.object_pose)      
             
        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. This sentence is from category 3"
            speak = speaker(tosay)
            speak.execute() 
            
            return "aborted"
    #############################################################################
    time.sleep(SLEEP_TIME)
    return "succeeded"