def call_bring_to(person_name): #TODO #Adding realese and reread tosay with some responsible person if person_name == '': tosay = "I'm leaving this here, sorry but you asked for a person without name" else: tosay = person_name + " can you pick this up please?" speak = speaker(tosay) speak.execute() rospy.logwarn('call_bring_to '+person_name) ############################################################################# if SKILLS : if (time.time()-TIME_INIT) > 270: return "succeeded" out = 'aborted' tries = 0 while(out=='aborted' and tries<3): person_object_position = PoseStamped() person_object_position.header.frame_id = "base_link" person_object_position.pose.position.x = 0.25 person_object_position.pose.position.z = 1.0 person_object_position.pose.orientation.w = 1.0 sm = place_object_sm(person_object_position) out = sm.execute() tries = tries+1 #realese here ############################################################################# time.sleep(SLEEP_TIME) return "succeeded"
def call_bring_to( person_name ): #TODO #Adding realese and reread tosay with some responsible person if person_name == '': tosay = "I'm leaving this here, sorry but you asked for a person without name" else: tosay = person_name + " can you pick this up please?" speak = speaker(tosay) speak.execute() rospy.logwarn('call_bring_to ' + person_name) ############################################################################# if SKILLS: if (time.time() - TIME_INIT) > 270: return "succeeded" out = 'aborted' tries = 0 while (out == 'aborted' and tries < 3): person_object_position = PoseStamped() person_object_position.header.frame_id = "base_link" person_object_position.pose.position.x = 0.25 person_object_position.pose.position.z = 1.0 person_object_position.pose.orientation.w = 1.0 sm = place_object_sm(person_object_position) out = sm.execute() tries = tries + 1 #realese here ############################################################################# time.sleep(SLEEP_TIME) return "succeeded"
def call_bring_to_loc(location_name): #TODO #Improve toSay, add realese and, may be add some human recognition to avoid throwing stuff to the ground if location_name == '': tosay = "I'm leaving this here" else: tosay = "I took this item here as requested. Referee I know you are here, if no one else is going to pick this proably you will want to take it before I throw it to the floor, thanks" speak = speaker(tosay) speak.execute() rospy.logwarn('call_bring_to_loc '+location_name) ############################################################################# if SKILLS : if (time.time()-TIME_INIT) > 270: return "succeeded" param_name = "/robocup_params/" + location_name.replace(" ","_") + "_heigh" loc_object_position = PoseStamped() loc_object_position.header.frame_id = "base_link" loc_object_position.pose.position.x = 0.5 if rospy.has_param(param_name): loc_object_position.pose.position.z = rospy.get_param(param_name) else: loc_object_position.pose.position.z = 1.25 loc_object_position.pose.orientation.w = 1.0 out = 'aborted' tries = 0 while(out=='aborted' and tries<3): sm = place_object_sm(loc_object_position) out = sm.execute() tries = tries+1 #realese here ############################################################################# time.sleep(SLEEP_TIME) return "succeeded"
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, ['succeeded', 'preempted', 'aborted'], input_keys=['person_location', 'person_location_coord']) 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: self.userdata.emergency_location = [] self.userdata.tts_lang = 'en_US' self.userdata.tts_wait_before_speaking = 0 self.userdata.object_failed = False self.userdata.object_name = None smach.StateMachine.add( 'Ask_Question', text_to_say(text='What would you like me to bring?'), transitions={'succeeded':'Listen_Question', 'aborted': 'Ask_Question', 'preempted':'Listen_Question'}) smach.StateMachine.add( 'Listen_Question', ListenToSM(grammar='robocup/emergency'), transitions={'succeeded':'Process_Tags', 'aborted':'Ask_Question', 'preempted':'Ask_Question'}) # Get the output from AskQuestionSM, process it, and search in the yaml file for the location of the object asked # Input keys: actiontag[] 'asr_userSaid_tags' # Output keys: object smach.StateMachine.add( 'Process_Tags', Process_Tags(), transitions={'succeeded':'Search_Object', 'aborted':'Ask_Question', 'aborted':'Ask_Question'}) smach.StateMachine.add( 'Search_Object', object_detection_and_grasping_sm(), transitions={'succeeded':'Say_return_Person', 'fail_object_detection':'Grasp_failed_prepare', 'fail_object_grasping':'Grasp_failed_prepare', 'aborted':'aborted', 'preempted':'preempted'}, remapping = {'object_name':'object_to_grasp'}) smach.StateMachine.add( 'Grasp_failed_prepare', Fail_Detection_Grasping(), transitions={'succeeded':'Grasp_fail_Ask_Person'}) # TODO: Saying to where to robot is heading. # smach.StateMachine.add( # 'Say_go_Place', # text_to_say('I am Going to the Kitchen for an object, Stay Here until I give you the object'), # transitions={'succeeded':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place'}) # smach.StateMachine.add( # 'Go_To_Object_Place', # nav_to_poi(), # transitions={'succeeded':'Say_got_to_Kitchen', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'Grasp_fail_Ask_Person'}) # smach.StateMachine.add( # 'Say_got_to_Kitchen', # text_to_say('I am in the Kitchen, I am going to grasp fail ask person'), # transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person'}) # # self.userdata.time_grasp = 0.0 # smach.StateMachine.add('Grasping_with_timeout', # grasping_with_timeout(), # transitions={'succeeded':'Prepare_Go_To_Person', 'time_out':'Grasp_fail_Ask_Person'}) smach.StateMachine.add( 'Grasp_fail_Ask_Person', ask_give_object_grasping(), transitions={'succeeded':'Rest_arm', 'aborted':'Rest_arm', 'preempted':'Rest_arm'}) smach.StateMachine.add( 'Rest_arm', play_motion_sm('rest_object_right'), transitions={'succeeded':'Say_return_Person', 'aborted':'Say_return_Person', 'preempted':'Say_return_Person'}) smach.StateMachine.add( 'Say_return_Person', text_to_say('I am preparing to go back to the person'), transitions={'succeeded':'Prepare_Go_To_Person', 'aborted':'Prepare_Go_To_Person', 'aborted':'Prepare_Go_To_Person'}) #Go to person smach.StateMachine.add( 'Prepare_Go_To_Person', prepare_poi_person_emergency(), transitions={'succeeded':'Go_To_Person', 'aborted':'Go_To_Person', 'preempted':'Go_To_Person'}) #TODO: POI For Person in Emergency -- From SearchPeople SM - smach.StateMachine.add( 'Go_To_Person', nav_to_coord('/map'), transitions={'succeeded':'Say_Give_Object', 'aborted':'Say_Give_Object', 'preempted':'Say_Give_Object'}) smach.StateMachine.add( 'Say_Give_Object', text_to_say('I am going to give you the Object you asked.'), transitions={'succeeded':'Select_next_state_grasping', 'aborted':'Select_next_state_grasping', 'preempted':'Select_next_state_grasping'}) smach.StateMachine.add( 'Select_next_state_grasping', Select_Grasp_Next_State(), transitions={'state_failed':'Give_object_arm', 'state_succeeded':'Give_object_both'}) smach.StateMachine.add( 'Give_object_both', place_object_sm(pose_place), transitions={'succeeded':'Say_Rescue_stay', 'aborted':'Say_Rescue_stay', 'preempted':'preempted'}) smach.StateMachine.add( 'Give_object_arm', play_motion_sm('give_object_right'), transitions={'succeeded':'Give_Object', 'aborted':'Give_Object', 'preempted':'Give_Object'}) #Give the grabbed object to the person smach.StateMachine.add( 'Give_Object', move_hands_form(hand_pose_name='pre_grasp', hand_side='right'), transitions={'succeeded':'Give_Object_2', 'aborted':'Give_Object', 'preempted':'Give_Object'}) smach.StateMachine.add( 'Give_Object_2', move_hands_form(hand_pose_name='full_open', hand_side='right'), transitions={'succeeded':'Say_Rescue_stay', 'aborted':'Give_Object_2', 'preempted':'Give_Object_2'}) smach.StateMachine.add( 'Say_Rescue_stay', text_to_say('Please Stay here I am going to call for the Ambulance'), transitions={'succeeded':'succeeded', 'aborted':'aborted', 'aborted':'preempted'})
def call_bring_to_loc( location_name ): #TODO #Improve toSay, add realese and, may be add some human recognition to avoid throwing stuff to the ground # if location_name == '': tosay = "I'm leaving this here" # else: # tosay = "I took this item here as requested. Referee I know you are here, if no one else is going to pick this proably you will want to take it before I throw it to the floor, thanks" speak = speaker(tosay) speak.execute() rospy.logwarn('call_bring_to_loc ' + location_name) ############################################################################# if SKILLS: if (time.time() - TIME_INIT) > 270: return "succeeded" #param_place_high_name = "/robocup_params/" + location_name.replace(" ","_") + "_heigh" param_place_high_name = "/robocup_params/place/" + location_name.replace( " ", "_") loc_object_position = PoseStamped() #PoseWithCovarianceStamped() loc_object_position.header.frame_id = "base_link" loc_object_position.pose.position.z = 1.25 loc_object_position.pose.position.x = 0.25 loc_object_position.pose.orientation.w = 1.0 current_position = world.get_current_position() rospy.logwarn(current_position) rospy.logwarn(ROOMS) if current_position == "kitchen": loc_object_position.pose.position.z = rospy.get_param( "/robocup_params/place/bar")[3] loc_object_position.pose.position.x = rospy.get_param( "/robocup_params/place/bar")[2] call_go_to("bar", world) m = place_object_sm(loc_object_position) out = sm.execute() if current_position == "living room": loc_object_position.pose.position.z = rospy.get_param( "/robocup_params/place/dinner_table")[3] loc_object_position.pose.position.x = rospy.get_param( "/robocup_params/place/dinner_table")[2] call_go_to("dinner table", world) m = place_object_sm(loc_object_position) out = sm.execute() if current_position == "hallway": loc_object_position.pose.position.z = rospy.get_param( "/robocup_params/place/hallway_table")[3] loc_object_position.pose.position.x = rospy.get_param( "/robocup_params/place/hallway_table")[2] call_go_to("hallway table", world) m = place_object_sm(loc_object_position) out = sm.execute() if current_position == "bedroom": loc_object_position.pose.position.z = rospy.get_param( "/robocup_params/place/bed")[3] loc_object_position.pose.position.x = rospy.get_param( "/robocup_params/place/bed")[2] call_go_to("bed", world) m = place_object_sm(loc_object_position) out = sm.execute() if current_position.replace("_", " ") in TABLES: loc_object_position.pose.position.z = rospy.get_param( "/robocup_params/place/" + current_position.replace("_", " "))[3] loc_object_position.pose.position.x = rospy.get_param( "/robocup_params/place/" + current_position.replace("_", " "))[2] sm = place_object_sm(loc_object_position) out = sm.execute() ############################################################################# time.sleep(SLEEP_TIME) return "succeeded"
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, ['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 call_bring_to_loc(location_name): #TODO #Improve toSay, add realese and, may be add some human recognition to avoid throwing stuff to the ground # if location_name == '': tosay = "I'm leaving this here" # else: # tosay = "I took this item here as requested. Referee I know you are here, if no one else is going to pick this proably you will want to take it before I throw it to the floor, thanks" speak = speaker(tosay) speak.execute() rospy.logwarn('call_bring_to_loc '+location_name) ############################################################################# if SKILLS : if (time.time()-TIME_INIT) > 270: return "succeeded" #param_place_high_name = "/robocup_params/" + location_name.replace(" ","_") + "_heigh" param_place_high_name = "/robocup_params/place/" + location_name.replace(" ","_") loc_object_position = PoseStamped() #PoseWithCovarianceStamped() loc_object_position.header.frame_id = "base_link" loc_object_position.pose.position.z = 1.25 loc_object_position.pose.position.x = 0.25 loc_object_position.pose.orientation.w = 1.0 current_position = world.get_current_position() rospy.logwarn(current_position) rospy.logwarn(ROOMS) if current_position == "kitchen": loc_object_position.pose.position.z = rospy.get_param("/robocup_params/place/bar")[3] loc_object_position.pose.position.x = rospy.get_param("/robocup_params/place/bar")[2] call_go_to("bar",world) m = place_object_sm(loc_object_position) out = sm.execute() if current_position == "living room": loc_object_position.pose.position.z = rospy.get_param("/robocup_params/place/dinner_table")[3] loc_object_position.pose.position.x = rospy.get_param("/robocup_params/place/dinner_table")[2] call_go_to("dinner table",world) m = place_object_sm(loc_object_position) out = sm.execute() if current_position == "hallway": loc_object_position.pose.position.z = rospy.get_param("/robocup_params/place/hallway_table")[3] loc_object_position.pose.position.x = rospy.get_param("/robocup_params/place/hallway_table")[2] call_go_to("hallway table",world) m = place_object_sm(loc_object_position) out = sm.execute() if current_position == "bedroom": loc_object_position.pose.position.z = rospy.get_param("/robocup_params/place/bed")[3] loc_object_position.pose.position.x = rospy.get_param("/robocup_params/place/bed")[2] call_go_to("bed",world) m = place_object_sm(loc_object_position) out = sm.execute() if current_position.replace("_"," ") in TABLES: loc_object_position.pose.position.z = rospy.get_param("/robocup_params/place/"+current_position.replace("_"," "))[3] loc_object_position.pose.position.x = rospy.get_param("/robocup_params/place/"+current_position.replace("_"," "))[2] sm = place_object_sm(loc_object_position) out = sm.execute() ############################################################################# time.sleep(SLEEP_TIME) return "succeeded"
def __init__(self): smach.StateMachine.__init__( self, ['succeeded', 'preempted', 'aborted'], input_keys=['person_location', 'person_location_coord']) 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: self.userdata.emergency_location = [] self.userdata.tts_lang = 'en_US' self.userdata.tts_wait_before_speaking = 0 self.userdata.object_failed = False self.userdata.object_name = None smach.StateMachine.add( 'Ask_Question', text_to_say(text='What would you like me to bring?'), transitions={ 'succeeded': 'Listen_Question', 'aborted': 'Ask_Question', 'preempted': 'Listen_Question' }) smach.StateMachine.add('Listen_Question', ListenToSM(grammar='robocup/emergency'), transitions={ 'succeeded': 'Process_Tags', 'aborted': 'Ask_Question', 'preempted': 'Ask_Question' }) # Get the output from AskQuestionSM, process it, and search in the yaml file for the location of the object asked # Input keys: actiontag[] 'asr_userSaid_tags' # Output keys: object smach.StateMachine.add('Process_Tags', Process_Tags(), transitions={ 'succeeded': 'Search_Object', 'aborted': 'Ask_Question', 'aborted': 'Ask_Question' }) smach.StateMachine.add( 'Search_Object', object_detection_and_grasping_sm(), transitions={ 'succeeded': 'Say_return_Person', 'fail_object_detection': 'Grasp_failed_prepare', 'fail_object_grasping': 'Grasp_failed_prepare', 'aborted': 'aborted', 'preempted': 'preempted' }, remapping={'object_name': 'object_to_grasp'}) smach.StateMachine.add( 'Grasp_failed_prepare', Fail_Detection_Grasping(), transitions={'succeeded': 'Grasp_fail_Ask_Person'}) # TODO: Saying to where to robot is heading. # smach.StateMachine.add( # 'Say_go_Place', # text_to_say('I am Going to the Kitchen for an object, Stay Here until I give you the object'), # transitions={'succeeded':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place'}) # smach.StateMachine.add( # 'Go_To_Object_Place', # nav_to_poi(), # transitions={'succeeded':'Say_got_to_Kitchen', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'Grasp_fail_Ask_Person'}) # smach.StateMachine.add( # 'Say_got_to_Kitchen', # text_to_say('I am in the Kitchen, I am going to grasp fail ask person'), # transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person'}) # # self.userdata.time_grasp = 0.0 # smach.StateMachine.add('Grasping_with_timeout', # grasping_with_timeout(), # transitions={'succeeded':'Prepare_Go_To_Person', 'time_out':'Grasp_fail_Ask_Person'}) smach.StateMachine.add('Grasp_fail_Ask_Person', ask_give_object_grasping(), transitions={ 'succeeded': 'Rest_arm', 'aborted': 'Rest_arm', 'preempted': 'Rest_arm' }) smach.StateMachine.add('Rest_arm', play_motion_sm('rest_object_right'), transitions={ 'succeeded': 'Say_return_Person', 'aborted': 'Say_return_Person', 'preempted': 'Say_return_Person' }) smach.StateMachine.add( 'Say_return_Person', text_to_say('I am preparing to go back to the person'), transitions={ 'succeeded': 'Prepare_Go_To_Person', 'aborted': 'Prepare_Go_To_Person', 'aborted': 'Prepare_Go_To_Person' }) #Go to person smach.StateMachine.add('Prepare_Go_To_Person', prepare_poi_person_emergency(), transitions={ 'succeeded': 'Go_To_Person', 'aborted': 'Go_To_Person', 'preempted': 'Go_To_Person' }) #TODO: POI For Person in Emergency -- From SearchPeople SM - smach.StateMachine.add('Go_To_Person', nav_to_coord('/map'), transitions={ 'succeeded': 'Say_Give_Object', 'aborted': 'Say_Give_Object', 'preempted': 'Say_Give_Object' }) smach.StateMachine.add( 'Say_Give_Object', text_to_say('I am going to give you the Object you asked.'), transitions={ 'succeeded': 'Select_next_state_grasping', 'aborted': 'Select_next_state_grasping', 'preempted': 'Select_next_state_grasping' }) smach.StateMachine.add('Select_next_state_grasping', Select_Grasp_Next_State(), transitions={ 'state_failed': 'Give_object_arm', 'state_succeeded': 'Give_object_both' }) smach.StateMachine.add('Give_object_both', place_object_sm(pose_place), transitions={ 'succeeded': 'Say_Rescue_stay', 'aborted': 'Say_Rescue_stay', 'preempted': 'preempted' }) smach.StateMachine.add('Give_object_arm', play_motion_sm('give_object_right'), transitions={ 'succeeded': 'Give_Object', 'aborted': 'Give_Object', 'preempted': 'Give_Object' }) #Give the grabbed object to the person smach.StateMachine.add('Give_Object', move_hands_form(hand_pose_name='pre_grasp', hand_side='right'), transitions={ 'succeeded': 'Give_Object_2', 'aborted': 'Give_Object', 'preempted': 'Give_Object' }) smach.StateMachine.add('Give_Object_2', move_hands_form(hand_pose_name='full_open', hand_side='right'), transitions={ 'succeeded': 'Say_Rescue_stay', 'aborted': 'Give_Object_2', 'preempted': 'Give_Object_2' }) smach.StateMachine.add( 'Say_Rescue_stay', text_to_say( 'Please Stay here I am going to call for the Ambulance'), transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'aborted': '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.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': 'Search_and_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'}) # 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'})
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, ['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.loop_iterations = 0 self.userdata.try_iterations = 1 self.userdata.gesture_name = '' self.userdata.object_name = "" self.userdata.manip_time_to_play = 4 self.userdata.did_pick = True self.userdata.grammar_name = GRAMMAR_NAME smach.StateMachine.add( 'init_cocktail', text_to_say("Ready for cocktail party"), transitions={'succeeded': 'learning_person', 'aborted': 'wait_for_door'}) # We wait for open door and go inside smach.StateMachine.add( 'wait_for_door', EnterRoomSM("party_room"), transitions={'succeeded': 'say_search_wave', 'aborted': 'aborted', 'preempted': 'preempted'}) # Say Wave recognize smach.StateMachine.add( 'say_search_wave', text_to_say("I'm searching for people waving at me", wait=False), transitions={'succeeded': 'wave_recognition', 'aborted': 'wave_recognition'}) # Gesture recognition -> Is anyone waving? smach.StateMachine.add( 'wave_recognition', WaveDetection(), transitions={'succeeded': 'say_wave_recognize', 'aborted': 'ask_for_person', 'preempted': 'preempted'}) # Say Wave recognize smach.StateMachine.add( 'say_wave_recognize', text_to_say("Someone waved to me. I will go there", wait=False), transitions={'succeeded': 'prepare_coord_wave', 'aborted': 'prepare_coord_wave'}) # Prepare the goal to the person that is waving # TODO: it goes a little far to the person... smach.StateMachine.add( 'prepare_coord_wave', prepare_coord_wave(), transitions={'succeeded': 'go_to_person_wave', 'aborted': 'aborted', 'preempted': 'preempted'}) # Go to the person -> we assume that gesture will return the position smach.StateMachine.add( 'go_to_person_wave', nav_to_coord('/base_link'), transitions={'succeeded': 'learning_person', 'aborted': 'go_to_person_wave', 'preempted': 'preempted'}) # Ask for person if it can see anyone smach.StateMachine.add( 'ask_for_person', text_to_say("I can't see anyone. Can anyone come to me, please?"), transitions={'succeeded': 'wait_for_person', 'aborted': 'ask_for_person', 'preempted': 'preempted'}) # Wait for person smach.StateMachine.add( 'wait_for_person', detect_face(), transitions={'succeeded': 'learning_person', 'aborted': 'ask_for_person'}) # Learn Person -> Ask name + Face Recognition # TODO: Set database smach.StateMachine.add( 'learning_person', SaveFaceSM(), transitions={'succeeded': 'ask_order', 'aborted': 'learning_person', 'preempted': 'preempted'}) # Ask for order smach.StateMachine.add( 'ask_order', AskQuestionSM("What would you like to order?", GRAMMAR_NAME), transitions={'succeeded': 'process_order', 'aborted': 'ask_order', 'preempted': 'preempted'}) # Process the answer smach.StateMachine.add( 'process_order', process_order(), transitions={'succeeded': 'say_got_it', 'aborted': 'ask_order', 'preempted': 'preempted'}) # Say what he ask smach.StateMachine.add( 'say_got_it', text_to_say("I got it!"), transitions={'succeeded': 'go_to_storage', 'aborted': 'go_to_storage', 'preempted': 'preempted'}) # Go to the storage_room smach.StateMachine.add( 'go_to_storage', nav_to_poi("storage_room"), transitions={'succeeded': 'recognize_object_and_pick', 'aborted': 'go_to_storage', 'preempted': 'preempted'}) # Recognize and pick object if found smach.StateMachine.add( 'recognize_object_and_pick', RecObjectAndPick(), transitions={'succeeded': 'go_to_party', 'fail_grasp':'go_to_party', 'fail_recognize': 'try_again_recognition'}) # 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_order', # text_to_say("I'm going to grasp it"), # transitions={'succeeded': 'grasp_food_order', 'aborted': 'Grasp_fail_Ask_Person', # 'preempted': 'preempted'}) # # # Grasp Object # smach.StateMachine.add( # 'grasp_food_order', # pick_object_sm(), # transitions={'succeeded': 'go_to_party', 'aborted': 'Grasp_fail_Ask_Person', # 'preempted': 'preempted'}) # We don't recognized the object smach.StateMachine.add( 'try_again_recognition', checkRecognition(), 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."), transitions={'succeeded': 'Grasp_fail_Ask_Person', 'aborted': 'Grasp_fail_Ask_Person'}) # 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':'go_to_party', 'aborted':'go_to_party', 'preempted':'go_to_party'}) # Go to the party room smach.StateMachine.add( 'go_to_party', nav_to_poi('party_room'), transitions={'succeeded': 'say_search_person', 'aborted': 'go_to_party', 'preempted': 'preempted'}) # Say search for person smach.StateMachine.add( 'say_search_person', text_to_say("I'm going to search the person who ordered me"), transitions={'succeeded': 'search_for_person', 'aborted': 'search_for_person', 'preempted': 'preempted'}) # Search for person -> He could change his position smach.StateMachine.add( 'search_for_person', recognize_face_concurrent(), transitions={'succeeded': 'say_found_person', 'aborted': 'prepare_ask_for_person_back', 'preempted': 'preempted'}) # Say found the person smach.StateMachine.add( 'say_found_person', text_to_say("I found you!"), transitions={'succeeded': 'prepare_coord_order', 'aborted': 'aborted', 'preempted': 'preempted'}) # Prepare the goal to the person that ask for the order smach.StateMachine.add( 'prepare_coord_order', prepare_coord_order(), transitions={'succeeded': 'go_to_person_order', 'aborted': 'aborted', 'preempted': 'preempted'}) # Go to person smach.StateMachine.add( 'go_to_person_order', nav_to_coord('/base_link'), transitions={'succeeded': 'deliver_drink', 'aborted': 'aborted', 'preempted': 'preempted'}) # Ask for person if it can see anyone smach.StateMachine.add( 'prepare_ask_for_person_back', prepare_ask_person_back(), transitions={'succeeded': 'ask_for_person_back', 'aborted': 'aborted', 'preempted': 'preempted'}) smach.StateMachine.add( 'ask_for_person_back', text_to_say(), transitions={'succeeded': 'deliver_drink', 'aborted': 'aborted', 'preempted': 'preempted'}) # Deliver Drink smach.StateMachine.add( 'deliver_drink', text_to_say("I'm going to deliver the drink"), transitions={'succeeded': 'Give_Object', 'aborted': 'Give_Object', 'preempted': 'preempted'}) # Check if we pick or ask the drink smach.StateMachine.add( 'check_place_give', check_place_give(), transitions={'succeeded':'Place_Object', 'aborted':'Give_Object', 'preempted':'check_place_give'}) # Place if we pick the drink smach.StateMachine.add( 'Place_Object', place_object_sm(), transitions={'succeeded':'check_loop', 'aborted':'Place_Object', 'preempted':'Place_Object'}) # Give if we ask for drink smach.StateMachine.add( 'Give_Object', give_object(), transitions={'succeeded':'check_loop', 'aborted':'Give_Object', 'preempted':'Give_Object'}) # End of loop? smach.StateMachine.add( 'check_loop', checkLoop(), transitions={'succeeded': 'wave_recognition', 'aborted': 'aborted', 'preempted': 'preempted', 'end':'leaving_arena'}) # Say leaving the arena smach.StateMachine.add( 'say_leaving_arena', text_to_say("I finished the cocktail party, I'm leaving the arena"), transitions={'succeeded': 'Give_Object', 'aborted': 'Give_Object', 'preempted': 'preempted'}) # Leaving the arena smach.StateMachine.add( 'leaving_arena', nav_to_poi('leave_arena'), transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})