def __init__(self): smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted', 'fail_grasp', 'fail_recognize'], input_keys=['object_name'], output_keys=['object_position','object_detected_name']) with self: # We must initialize the userdata keys if they are going to be accessed or they won't exist and crash! self.userdata.nav_to_poi_name='' self.userdata.tts_lang = '' self.userdata.tts_wait_before_speak = '' self.userdata.tts_text = '' # Say start object recognition smach.StateMachine.add( 'say_start_obj_recognition', text_to_say("I'm going to start the Object recognition process.", wait=False), transitions={'succeeded': 'object_recognition', 'aborted': 'object_recognition'}) # Do object_recognition smach.StateMachine.add( 'object_recognition', recognize_object(), transitions={'succeeded': 'process_object_recognition', 'aborted': 'fail_recognize', 'preempted': 'preempted'}) # Process the objects recognized smach.StateMachine.add( 'process_object_recognition', process_pick_location(), transitions={'succeeded': 'say_grasp_object', 'aborted': 'fail_recognize', 'preempted': 'preempted'}) # Say grasp object smach.StateMachine.add( 'say_grasp_object', text_to_say("I'm going to grasp the object", wait=False), transitions={'succeeded': 'grasp_object', 'aborted': 'aborted'}) # Grasp the object smach.StateMachine.add( 'grasp_object', pick_object_sm(), transitions={'succeeded': 'succeeded', 'aborted': 'home_position', 'preempted': 'preempted'}) # Home position smach.StateMachine.add( 'home_position', play_motion_sm('home',skip_planning=True), transitions={'succeeded': 'fail_grasp', 'aborted': 'home_position', 'preempted': 'preempted'})
def __init__(self, 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' })
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"
def __init__(self): smach.StateMachine.__init__( self, outcomes=[ 'succeeded', 'preempted', 'aborted', 'fail_grasp', 'fail_recognize' ], input_keys=['object_name'], output_keys=['object_position', 'object_detected_name']) with self: # We must initialize the userdata keys if they are going to be accessed or they won't exist and crash! self.userdata.nav_to_poi_name = '' self.userdata.tts_lang = '' self.userdata.tts_wait_before_speak = '' self.userdata.tts_text = '' # Say start object recognition smach.StateMachine.add( 'say_start_obj_recognition', text_to_say( "I'm going to start the Object recognition process.", wait=False), transitions={ 'succeeded': 'object_recognition', 'aborted': 'object_recognition' }) # Do object_recognition smach.StateMachine.add('object_recognition', recognize_object(), transitions={ 'succeeded': 'process_object_recognition', 'aborted': 'fail_recognize', 'preempted': 'preempted' }) # Process the objects recognized smach.StateMachine.add('process_object_recognition', process_pick_location(), transitions={ 'succeeded': 'say_grasp_object', 'aborted': 'fail_recognize', 'preempted': 'preempted' }) # Say grasp object smach.StateMachine.add('say_grasp_object', text_to_say("I'm going to grasp the object", wait=False), transitions={ 'succeeded': 'grasp_object', 'aborted': 'aborted' }) # Grasp the object smach.StateMachine.add('grasp_object', pick_object_sm(), transitions={ 'succeeded': 'succeeded', 'aborted': 'home_position', 'preempted': 'preempted' }) # Home position smach.StateMachine.add('home_position', play_motion_sm('home', skip_planning=True), transitions={ 'succeeded': 'fail_grasp', 'aborted': 'home_position', 'preempted': 'preempted' })
def __init__(self, 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'})
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"