def call_grasp(obj,world): #TODO #adding grasping tosay = "I'm going to grasp the " + obj speak = speaker(tosay) speak.execute() rospy.logwarn('call_grasp '+obj) rospy.logwarn(world.item.object_pose) ############################################################################# if SKILLS : pose_object_to_grasp = PoseStamped() pose_object_to_grasp.header = world.item.object_pose.header pose_object_to_grasp.pose = world.item.object_pose.pose.pose if (time.time()-TIME_INIT) > 270: return "succeeded" out = 'aborted' tries = 0 while(out=='aborted' and tries<3): sm = pick_object_sm(pose_object_to_grasp) #if not workng, blame chang out = sm.execute() tries = tries+1 #grasping here ############################################################################# time.sleep(SLEEP_TIME) return "succeeded"
def call_grasp(obj, world): #TODO #adding grasping tosay = "I'm going to grasp the " + obj speak = speaker(tosay) speak.execute() rospy.logwarn('call_grasp ' + obj) rospy.logwarn(world.item.object_pose) ############################################################################# if SKILLS: pose_object_to_grasp = PoseStamped() pose_object_to_grasp.header = world.item.object_pose.header pose_object_to_grasp.pose = world.item.object_pose.pose.pose if (time.time() - TIME_INIT) > 270: return "succeeded" out = 'aborted' tries = 0 while (out == 'aborted' and tries < 3): sm = pick_object_sm( pose_object_to_grasp) #if not workng, blame chang out = sm.execute() tries = tries + 1 #grasping here ############################################################################# time.sleep(SLEEP_TIME) return "succeeded"
def __init__(self): smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted', 'fail_object_detection', 'fail_object_grasping'], input_keys=['object_name', 'time_out_grasp'], output_keys=['standard_error', 'object_detected_name']) with self: self.userdata.object_detected_name = '' smach.StateMachine.add('Search_Object', SearchObjectSM(), transitions={'succeeded':'Grasp_object', 'aborted':'fail_object_detection', 'preempted':'preempted'}) smach.StateMachine.add('Prepare_Grasp', prepare_grasp(), transitions={'succeeded':'Grasp_object', 'aborted':'aborted'}) smach.StateMachine.add('Grasp_object', pick_object_sm(), transitions={'succeeded':'succeeded', 'aborted':'fail_object_grasping', 'preempted':'preempted'})
def __init__(self): smach.StateMachine.__init__( self, outcomes=[ 'succeeded', 'aborted', 'preempted', 'fail_object_detection', 'fail_object_grasping' ], input_keys=['object_name', 'time_out_grasp'], output_keys=['standard_error', 'object_detected_name']) with self: self.userdata.object_detected_name = '' smach.StateMachine.add('Search_Object', SearchObjectSM(), transitions={ 'succeeded': 'Grasp_object', 'aborted': 'fail_object_detection', 'preempted': 'preempted' }) smach.StateMachine.add('Prepare_Grasp', prepare_grasp(), transitions={ 'succeeded': 'Grasp_object', 'aborted': 'aborted' }) smach.StateMachine.add('Grasp_object', pick_object_sm(), transitions={ 'succeeded': 'succeeded', 'aborted': 'fail_object_grasping', 'preempted': 'preempted' })
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): smach.StateMachine.__init__( self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['object_array']) pose_place = PoseStamped() pose_place.header.frame_id = '/base_link' pose_place.pose.position.x = 0.90 pose_place.pose.position.y = 0.0 pose_place.pose.position.z = 1.20 pose_place.pose.orientation.w = 1.0 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 self.userdata.object_name = None self.userdata.time_out_grasp = TIME_OUT_GRASP # Process order smach.StateMachine.add('decide_next_object', decide_next_object(), remapping={"object_loc": "nav_to_poi_name"}, transitions={ 'succeeded': 'say_im_going', 'aborted': 'aborted', 'preempted': 'preempted' }) # Deliver object smach.StateMachine.add('say_im_going', text_to_say(), 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(), transitions={ 'succeeded': 'say_grasp_object', 'aborted': 'go_to_object' }) # Grasp Object smach.StateMachine.add( 'say_grasp_object', text_to_say("I'm going to recognize and grasp the object"), transitions={ 'succeeded': 'recognize_object_and_pick', 'aborted': 'aborted', 'preempted': 'preempted' }) # smach.StateMachine.add( # 'Search_and_grasp_Object', # object_detection_and_grasping_sm(), # transitions={'succeeded':'prepare_delivery_goal', # 'fail_object_detection':'say_object_not_find', # 'fail_object_grasping':'say_not_grasp', # 'aborted':'aborted', # 'preempted':'preempted'}, # remapping = {'object_name':'object_to_grasp'}) # Recognize and pick object if found # Grasp the object # Grasp the object smach.StateMachine.add('grasp_object', pick_object_sm(), transitions={ 'succeeded': 'succeeded', 'aborted': 'say_not_grasp', 'preempted': 'preempted' }) smach.StateMachine.add('recognize_object_and_pick', RecObjectAndPick(), transitions={ 'succeeded': 'prepare_delivery_goal', 'fail_grasp': 'say_not_grasp', 'fail_recognize': 'say_object_not_find' }) # FAIL Grasp Object smach.StateMachine.add( 'say_not_grasp', text_to_say("it was impossible to grasp the Object"), transitions={ 'succeeded': 'decide_next_object', 'aborted': 'aborted', 'preempted': 'preempted' }) # FAIL Recognize Object smach.StateMachine.add( 'say_object_not_find', text_to_say("it was not possible to find the Object"), transitions={ 'succeeded': 'decide_next_object', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add('prepare_delivery_goal', prepare_delivery(), transitions={ 'succeeded': 'say_going_deliver_object', 'aborted': 'say_going_deliver_object', 'preempted': 'preempted' }) # Deliver object smach.StateMachine.add( 'say_going_deliver_object', text_to_say("i am going to deliver the object"), transitions={ 'succeeded': 'go_to_delivery', 'aborted': 'aborted', 'preempted': 'preempted' }) # Go to the delivery place smach.StateMachine.add('go_to_delivery', nav_to_poi(), transitions={ 'succeeded': 'say_deliver_object', 'aborted': 'aborted', 'preempted': 'preempted' }) # Deliver object smach.StateMachine.add( 'say_deliver_object', text_to_say("I'm going to deliver the object"), transitions={ 'succeeded': 'deliver_object', 'aborted': 'aborted', 'preempted': 'preempted' }) smach.StateMachine.add('deliver_object', place_object_sm(pose_place), transitions={ 'succeeded': 'check_loop', 'aborted': 'check_loop', 'preempted': 'preempted' }) # End of loop? smach.StateMachine.add('check_loop', checkLoop(), transitions={ 'succeeded': 'decide_next_object', 'aborted': 'aborted', 'preempted': 'preempted', 'end': '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): 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='' self.userdata.tts_lang = '' self.userdata.tts_wait_before_speak = '' self.userdata.tts_text = '' # Say start Pick and Place smach.StateMachine.add( 'say_start_pick_place', text_to_say("I'm going to start Pick and Place test"), transitions={'succeeded': 'say_start_obj_recognition', 'aborted': 'say_start_obj_recognition'}) # Say Going to location # smach.StateMachine.add( # 'say_go_location', # text_to_say("I'm going to the location where I have to recognize some objects",wait=False), # transitions={'succeeded': 'prepare_location', 'aborted': 'prepare_location'}) # # # Prepare the poi for nav_to_poi # smach.StateMachine.add( # 'prepare_location', # prepare_location(), # transitions={'succeeded': 'go_location', 'aborted': 'prepare_location', # 'preempted': 'preempted'}) # # # Go to the location # smach.StateMachine.add( # 'go_location', # nav_to_poi(), # transitions={'succeeded': 'say_start_obj_recognition', 'aborted': 'say_go_location', # 'preempted': 'preempted'}) # Say start object recognition smach.StateMachine.add( 'say_start_obj_recognition', text_to_say("I'm going to start the Object recognition"), transitions={'succeeded': 'object_recognition', 'aborted': 'object_recognition'}) # Do object_recognition smach.StateMachine.add( 'object_recognition', dummy_recognize(), transitions={'succeeded': 'say_grasp_object', 'aborted': 'say_release_obj', 'preempted': 'preempted'}) # Say grasp object smach.StateMachine.add( 'say_grasp_object', text_to_say("I'm going to grasp the object"), transitions={'succeeded': 'grasp_object', 'aborted': 'grasp_object'}) # Grasp the object smach.StateMachine.add( 'grasp_object', pick_object_sm(), transitions={'succeeded': 'say_go_second_location', 'aborted': 'play_motion_grasp', 'preempted': 'preempted'}) # Home position smach.StateMachine.add( 'play_motion_grasp', play_motion_sm('home'), transitions={'succeeded': 'say_grasp_object', 'preempted':'say_grasp_object', 'aborted':'play_motion_grasp'}) # Say go to second location smach.StateMachine.add( 'say_go_second_location', text_to_say("I'm going to the location where I should release the object"), transitions={'succeeded': 'go_second_location', 'aborted': 'go_second_location'}) # Go the location - We need to go to the place to object category, so we assume that the # object recognition will init the poi to the object must to go smach.StateMachine.add( 'go_second_location', nav_to_poi(), transitions={'succeeded': 'say_release_obj', 'aborted': 'say_go_second_location', 'preempted': 'preempted'}) # Say release object smach.StateMachine.add( 'say_release_obj', text_to_say("I'm going to release the object"), transitions={'succeeded': 'pregrasp_state', 'aborted': 'pregrasp_state'}) # Pre-grasp position smach.StateMachine.add( 'pregrasp_state', play_motion_sm('pre_grasp', skip_planning=True), transitions={'succeeded': 'play_motion_state', 'preempted':'play_motion_state', 'aborted':'pregrasp_state'}) # Release the object smach.StateMachine.add( 'release_object', place_object_sm(), transitions={'succeeded': 'play_motion_state', 'aborted': 'say_release_obj', 'preempted': 'preempted'}) # Home position smach.StateMachine.add( 'play_motion_state', play_motion_sm('home'), transitions={'succeeded': 'say_end_pick_place', 'preempted':'say_end_pick_place', 'aborted':'say_end_pick_place'}) # Say end Pick and Place smach.StateMachine.add( 'say_end_pick_place', text_to_say("I finished the Pick and Place test"), transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})
def __init__(self): smach.StateMachine.__init__( self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=[ 'object_name', 'name_face', 'list_orders', 'loop_iterations' ], output_keys=['did_pick', 'name_face']) with self: # We must initialize the userdata keys if they are going to be accessed or they won't exist and crash! self.userdata.try_iterations = 1 self.userdata.did_pick = True # State that prepare the information for pick - Mock recognition smach.StateMachine.add('object_recognition', dummy_recognize(), transitions={ 'succeeded': 'select_object', 'aborted': 'select_object', 'preempted': 'preempted' }) # Recognize and pick object if found smach.StateMachine.add('recognize_object_and_pick', RecObjectAndPick(), transitions={ 'succeeded': 'object_detected', 'fail_grasp': 'Grasp_fail_Ask_Person', 'fail_recognize': 'try_again_recognition' }) # Check which object I picked and prepare the person name smach.StateMachine.add('object_detected', object_detected(), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted' }) # Recognize Fail Part # We don't recognized the object smach.StateMachine.add('try_again_recognition', checkLoop(), transitions={ 'succeeded': 'recognize_object_and_pick', 'aborted': 'recognize_object_and_pick', 'preempted': 'preempted', 'end': 'Grasp_fail_Ask_Person' }) #Grasp Mock Part # Check which object I picked and prepare the person name smach.StateMachine.add('select_object', select_object(), transitions={ 'succeeded': 'play_motion_grasp', 'aborted': 'aborted' }) # Home position smach.StateMachine.add('play_motion_grasp', play_motion_sm('home'), transitions={ 'succeeded': 'say_grasp_object', 'preempted': 'say_grasp_object', 'aborted': 'play_motion_grasp' }) # Say grasp object smach.StateMachine.add( 'say_grasp_object', text_to_say("I'm going to grasp the object"), transitions={ 'succeeded': 'grasp_object', 'aborted': 'grasp_object' }) # Grasp the object smach.StateMachine.add('grasp_object', pick_object_sm(), transitions={ 'succeeded': 'succeeded', 'aborted': 'Grasp_fail_Ask_Person', 'preempted': 'preempted' }) # We ask for the object # Ask for grasp object smach.StateMachine.add( 'Grasp_fail_Ask_Person', ask_give_object_grasping(), remapping={'object_to_grasp': 'object_name'}, transitions={ 'succeeded': 'Rest_arm', 'aborted': 'Rest_arm', 'preempted': 'Rest_arm' }) smach.StateMachine.add('Rest_arm', play_motion_sm('rest_object_right'), transitions={ 'succeeded': 'change_did_pick', 'aborted': 'change_did_pick', 'preempted': 'change_did_pick' }) smach.StateMachine.add('change_did_pick', change_did_pick(), transitions={ 'succeeded': 'succeeded', 'aborted': 'succeeded', 'preempted': 'preempted' })
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! # Robot presentation: Little talk + wave gesture STATES = [ text_to_say("Hi everybody! My name is REEM."), play_motion_sm("wave") ] STATE_NAMES = ["say_presentation", "salute_wave"] outcome_map = { 'succeeded': { "say_presentation": 'succeeded', "salute_wave": 'succeeded' } } smach.StateMachine.add("robot_presentation", ConcurrenceRobocup(states=STATES, state_names=STATE_NAMES, outcome_map=outcome_map), transitions={ 'succeeded': 'object_recognition', 'aborted': "robot_presentation" }) # Do object_recognition smach.StateMachine.add('object_recognition', dummy_recognize(), transitions={ 'succeeded': 'say_grasp_object', 'aborted': 'say_grasp_object', 'preempted': 'preempted' }) # Say grasp object smach.StateMachine.add( 'say_grasp_object', text_to_say("I am hungry. I am going to pick the noodles"), transitions={ 'succeeded': 'grasp_object', 'aborted': 'grasp_object' }) # Grasp the object smach.StateMachine.add('grasp_object', pick_object_sm(), transitions={ 'succeeded': 'say_why_object', 'aborted': 'play_motion_state', 'preempted': 'preempted' }) # Say release object smach.StateMachine.add( 'say_why_object', text_to_say( "I am a robot, I can't eat this. I am so sad! I am going to leave the noodles in the table", wait=False), transitions={ 'succeeded': 'release_object', 'aborted': 'release_object' }) # Release the object smach.StateMachine.add('release_object', place_object_sm(), transitions={ 'succeeded': 'robot_finish', 'aborted': 'aborted', 'preempted': 'preempted' }) # Say hard job + bow STATES = [ text_to_say( "Uff, this was a hard job. Thank you very much for your attention" ), play_motion_sm("bow") ] STATE_NAMES = ["say_hard_job", "motion_bow"] outcome_map = { 'succeeded': { "say_hard_job": 'succeeded', "motion_bow": 'succeeded' } } smach.StateMachine.add("robot_finish", ConcurrenceRobocup(states=STATES, state_names=STATE_NAMES, outcome_map=outcome_map), transitions={ 'succeeded': 'say_rest', 'aborted': "say_rest" }) # Say go rest smach.StateMachine.add('say_rest', text_to_say( "I am going to rest for a few seconds", wait=False), transitions={ 'succeeded': 'sleep_robot', 'aborted': 'sleep_robot' }) # Sleep smach.StateMachine.add('sleep_robot', Sleeper(15), transitions={ 'succeeded': 'robot_presentation', 'preempted': 'robot_presentation', 'aborted': 'robot_presentation' }) # Home position smach.StateMachine.add('play_motion_state', play_motion_sm('home'), transitions={ 'succeeded': 'say_grasp_object', 'preempted': 'say_grasp_object', 'aborted': 'say_grasp_object' })
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! # Robot presentation: Little talk + wave gesture STATES = [text_to_say("Hi everybody! My name is REEM."), play_motion_sm("wave")] STATE_NAMES = ["say_presentation", "salute_wave"] outcome_map = {"succeeded": {"say_presentation": "succeeded", "salute_wave": "succeeded"}} smach.StateMachine.add( "robot_presentation", ConcurrenceRobocup(states=STATES, state_names=STATE_NAMES, outcome_map=outcome_map), transitions={"succeeded": "object_recognition", "aborted": "robot_presentation"}, ) # Do object_recognition smach.StateMachine.add( "object_recognition", dummy_recognize(), transitions={"succeeded": "say_grasp_object", "aborted": "say_grasp_object", "preempted": "preempted"}, ) # Say grasp object smach.StateMachine.add( "say_grasp_object", text_to_say("I am hungry. I am going to pick the noodles"), transitions={"succeeded": "grasp_object", "aborted": "grasp_object"}, ) # Grasp the object smach.StateMachine.add( "grasp_object", pick_object_sm(), transitions={"succeeded": "say_why_object", "aborted": "play_motion_state", "preempted": "preempted"}, ) # Say release object smach.StateMachine.add( "say_why_object", text_to_say( "I am a robot, I can't eat this. I am so sad! I am going to leave the noodles in the table", wait=False, ), transitions={"succeeded": "release_object", "aborted": "release_object"}, ) # Release the object smach.StateMachine.add( "release_object", place_object_sm(), transitions={"succeeded": "robot_finish", "aborted": "aborted", "preempted": "preempted"}, ) # Say hard job + bow STATES = [ text_to_say("Uff, this was a hard job. Thank you very much for your attention"), play_motion_sm("bow"), ] STATE_NAMES = ["say_hard_job", "motion_bow"] outcome_map = {"succeeded": {"say_hard_job": "succeeded", "motion_bow": "succeeded"}} smach.StateMachine.add( "robot_finish", ConcurrenceRobocup(states=STATES, state_names=STATE_NAMES, outcome_map=outcome_map), transitions={"succeeded": "say_rest", "aborted": "say_rest"}, ) # Say go rest smach.StateMachine.add( "say_rest", text_to_say("I am going to rest for a few seconds", wait=False), transitions={"succeeded": "sleep_robot", "aborted": "sleep_robot"}, ) # Sleep smach.StateMachine.add( "sleep_robot", Sleeper(15), transitions={ "succeeded": "robot_presentation", "preempted": "robot_presentation", "aborted": "robot_presentation", }, ) # Home position smach.StateMachine.add( "play_motion_state", play_motion_sm("home"), transitions={ "succeeded": "say_grasp_object", "preempted": "say_grasp_object", "aborted": "say_grasp_object", }, )
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='' self.userdata.tts_lang = '' self.userdata.tts_wait_before_speak = '' self.userdata.tts_text = '' self.userdata.loop_iterations = 1 self.userdata.did_unk = False self.userdata.object_name = ['Pringles', 'Barritas'] # Say start Pick and Place smach.StateMachine.add( 'say_start_pick_place', text_to_say("I'm going to start Pick and Place test"), transitions={'succeeded': 'say_go_location', 'aborted': 'say_go_location'}) # Say Going to location smach.StateMachine.add( 'say_go_location', text_to_say("I'm going to the location where I have to recognize some objects", wait=False), transitions={'succeeded': 'prepare_location', 'aborted': 'prepare_location'}) # Prepare the poi for nav_to_poi smach.StateMachine.add( 'prepare_location', prepare_location(), transitions={'succeeded': 'go_location', 'aborted': 'prepare_location', 'preempted': 'preempted'}) # Go to the location smach.StateMachine.add( 'go_location', nav_to_poi(), transitions={'succeeded': 'recognize_object_and_pick', 'aborted': 'say_go_location', 'preempted': 'preempted'}) # recognize and pick object if found smach.StateMachine.add( 'recognize_object_and_pick', RecObjectAndPick(), transitions={'succeeded': 'Process_Place_location', 'fail_grasp':'Process_Place_location', 'fail_recognize': 'try_again_recognition'}) # Prepare the place location smach.StateMachine.add( 'Process_Place_location', process_place_location(), transitions={'succeeded':'say_go_second_location', 'aborted':'aborted'}) # 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': 'try_again_recognition', # 'preempted': 'preempted'}) # # # Process the objects recognized # smach.StateMachine.add( # 'process_object_recognition', # process_place_location(), # transitions={'succeeded': 'say_grasp_object', 'aborted': 'say_grasp_object', # 'preempted': 'preempted'}) # We don't recognized the object smach.StateMachine.add( 'try_again_recognition', checkLoop(), transitions={'succeeded': 'recognize_object_and_pick', 'aborted': 'recognize_object_and_pick', 'preempted': 'preempted', 'end':'say_fail_recognize'}) # Say fail recognize objects smach.StateMachine.add( 'say_fail_recognize', text_to_say("I'm not able to recognized any object. I'm going to search for anything"), transitions={'succeeded': 'prepare_unk_object', 'aborted': 'prepare_unk_object'}) # Prepare goal to pick any object smach.StateMachine.add( 'prepare_unk_object', prepare_unk_object(), transitions={'succeeded': 'say_grasp_object', 'aborted': 'say_grasp_object'}) # 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': 'grasp_object'}) # Grasp the object smach.StateMachine.add( 'grasp_object', pick_object_sm(), transitions={'succeeded': 'say_go_second_location', 'aborted': 'home_position', #TODO: Change aborted to try again 'preempted': 'preempted'}) # Home position smach.StateMachine.add( 'home_position', play_motion_sm('home'), transitions={'succeeded': 'say_grasp_object', 'aborted': 'home_position', #TODO: Change aborted to try again 'preempted': 'preempted'}) # Say go to second location smach.StateMachine.add( 'say_go_second_location', text_to_say("I'm going to the location where I should release the object", wait=False), transitions={'succeeded': 'go_second_location', 'aborted': 'go_second_location'}) # Go the location - We need to go to the place to object category, so we assume that the # object recognition will init the poi to the object must to go smach.StateMachine.add( 'go_second_location', nav_to_poi(), transitions={'succeeded': 'say_release_obj', 'aborted': 'say_go_second_location', 'preempted': 'preempted'}) # Say release object smach.StateMachine.add( 'say_release_obj', text_to_say("I'm going to release the object", wait=True), transitions={'succeeded': 'release_object', 'aborted': 'release_object'}) # Check if we pick the know or the unk object smach.StateMachine.add( 'check_place_give', check_object(), transitions={'succeeded':'release_object', 'aborted':'pregrasp_state', 'preempted':'check_place_give'}) # Release the object smach.StateMachine.add( 'release_object', place_object_sm(), transitions={'succeeded': 'play_motion_state', 'aborted': 'say_release_obj', 'preempted': 'preempted'}) # Pre-grasp position smach.StateMachine.add( 'pregrasp_state', play_motion_sm('pre_grasp', skip_planning=True), transitions={'succeeded': 'play_motion_state', 'preempted':'play_motion_state', 'aborted':'pregrasp_state'}) # Home position smach.StateMachine.add( 'play_motion_state', play_motion_sm('home'), transitions={'succeeded': 'say_end_pick_place', 'preempted':'say_end_pick_place', 'aborted':'say_end_pick_place'}) # Say end Pick and Place smach.StateMachine.add( 'say_end_pick_place', text_to_say("I finished the Pick and Place test"), transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})
def __init__(self): smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['object_name', 'name_face', 'list_orders', 'loop_iterations'], output_keys=['did_pick','name_face']) with self: # We must initialize the userdata keys if they are going to be accessed or they won't exist and crash! self.userdata.try_iterations = 1 self.userdata.did_pick = True # State that prepare the information for pick - Mock recognition smach.StateMachine.add( 'object_recognition', dummy_recognize(), transitions={'succeeded': 'select_object', 'aborted': 'select_object', 'preempted': 'preempted'}) # Recognize and pick object if found smach.StateMachine.add( 'recognize_object_and_pick', RecObjectAndPick(), transitions={'succeeded': 'object_detected', 'fail_grasp':'Grasp_fail_Ask_Person', 'fail_recognize': 'try_again_recognition'}) # Check which object I picked and prepare the person name smach.StateMachine.add( 'object_detected', object_detected(), transitions={'succeeded': 'succeeded', 'aborted':'aborted'}) # Recognize Fail Part # We don't recognized the object smach.StateMachine.add( 'try_again_recognition', checkLoop(), transitions={'succeeded': 'recognize_object_and_pick', 'aborted': 'recognize_object_and_pick', 'preempted': 'preempted', 'end':'Grasp_fail_Ask_Person'}) #Grasp Mock Part # Check which object I picked and prepare the person name smach.StateMachine.add( 'select_object', select_object(), transitions={'succeeded': 'play_motion_grasp', 'aborted':'aborted'}) # Home position smach.StateMachine.add( 'play_motion_grasp', play_motion_sm('home'), transitions={'succeeded': 'say_grasp_object', 'preempted':'say_grasp_object', 'aborted':'play_motion_grasp'}) # Say grasp object smach.StateMachine.add( 'say_grasp_object', text_to_say("I'm going to grasp the object"), transitions={'succeeded': 'grasp_object', 'aborted': 'grasp_object'}) # Grasp the object smach.StateMachine.add( 'grasp_object', pick_object_sm(), transitions={'succeeded': 'succeeded', 'aborted': 'Grasp_fail_Ask_Person', 'preempted': 'preempted'}) # We ask for the object # Ask for grasp object smach.StateMachine.add( 'Grasp_fail_Ask_Person', ask_give_object_grasping(), remapping={'object_to_grasp':'object_name'}, transitions={'succeeded':'Rest_arm', 'aborted':'Rest_arm', 'preempted':'Rest_arm'}) smach.StateMachine.add( 'Rest_arm', play_motion_sm('rest_object_right'), transitions={'succeeded':'change_did_pick', 'aborted':'change_did_pick', 'preempted':'change_did_pick'}) smach.StateMachine.add( 'change_did_pick', change_did_pick(), transitions={'succeeded':'succeeded', 'aborted':'succeeded', 'preempted':'preempted'})
def __init__(self): smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['object_array']) pose_place = PoseStamped() pose_place.header.frame_id = '/base_link' pose_place.pose.position.x = 0.0 pose_place.pose.position.y = 0.0 pose_place.pose.position.z = 1.0 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 self.userdata.object_name=None self.userdata.time_out_grasp=TIME_OUT_GRASP # Process order smach.StateMachine.add( 'decide_next_object', decide_next_object(), remapping={"object_loc": "nav_to_poi_name"}, transitions={'succeeded': 'say_im_going', 'aborted': 'aborted', 'preempted': 'preempted'}) # Deliver object smach.StateMachine.add( 'say_im_going', text_to_say(), 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(), transitions={'succeeded': 'say_grasp_object', 'aborted': 'go_to_object'}) # Grasp Object smach.StateMachine.add( 'say_grasp_object', text_to_say("Grasping Object"), transitions={'succeeded': 'grasp_object', 'aborted': 'aborted', 'preempted': 'preempted'}) # smach.StateMachine.add( # 'Search_and_grasp_Object', # object_detection_and_grasping_sm(), # transitions={'succeeded':'prepare_delivery_goal', # 'fail_object_detection':'say_object_not_find', # 'fail_object_grasping':'say_not_grasp', # 'aborted':'aborted', # 'preempted':'preempted'}, # remapping = {'object_name':'object_to_grasp'}) # Recognize and pick object if found # Grasp the object # Grasp the object smach.StateMachine.add( 'grasp_object', pick_object_sm(), transitions={'succeeded': 'succeeded', 'aborted': 'say_not_grasp', 'preempted': 'preempted'}) smach.StateMachine.add( 'recognize_object_and_pick', RecObjectAndPick(), transitions={'succeeded': 'prepare_delivery_goal', 'fail_grasp':'say_not_grasp', 'fail_recognize': 'say_object_not_find'}) # Go to the delivery place # Grasp Object smach.StateMachine.add( 'say_not_grasp', text_to_say("it was impossible to grasp the Object"), transitions={'succeeded': 'decide_next_object', 'aborted': 'aborted', 'preempted': 'preempted'}) # Grasp Object smach.StateMachine.add( 'say_object_not_find', text_to_say("it was not possible to find the Object"), transitions={'succeeded': 'decide_next_object', 'aborted': 'aborted', 'preempted': 'preempted'}) smach.StateMachine.add( 'prepare_delivery_goal', prepare_delivery(), transitions={'succeeded': 'say_going_deliver_object', 'aborted': 'say_going_deliver_object', 'preempted': 'preempted'}) # Deliver object smach.StateMachine.add( 'say_going_deliver_object', text_to_say("i am going to deliver the object"), transitions={'succeeded': 'go_to_delivery', 'aborted': 'aborted', 'preempted': 'preempted'}) # Go to the delivery place smach.StateMachine.add( 'go_to_delivery', nav_to_poi(), transitions={'succeeded': 'say_deliver_object', 'aborted': 'aborted', 'preempted': 'preempted'}) # Deliver object smach.StateMachine.add( 'say_deliver_object', text_to_say("Delivering Object"), transitions={'succeeded': 'deliver_object', 'aborted': 'aborted', 'preempted': 'preempted'}) smach.StateMachine.add( 'deliver_object', place_object_sm(pose_place), transitions={'succeeded':'check_loop', 'aborted':'check_loop', 'preempted':'preempted'}) # End of loop? smach.StateMachine.add( 'check_loop', checkLoop(), transitions={'succeeded': 'decide_next_object', 'aborted': 'aborted', 'preempted': 'preempted', 'end':'succeeded'})