def __init__(self): smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'], output_keys=['standard_error']) with self: # Preparation of the Ask Process # Input Data: 'object_to_grasp' self.userdata.tts_wait_before_speaking = 0 self.userdata.tts_lang = 'en_US' smach.StateMachine.add('Ask_Person_Object', text_to_say('I am giving you the object you asked'), transitions={'succeeded':'Reach_Arm', 'preempted':'Reach_Arm', 'aborted':'Reach_Arm'}) # Reach the arm self.userdata.manip_motion_to_play = 'give_object_right' self.userdata.manip_time_to_play = 2.0 smach.StateMachine.add('Reach_Arm', play_motion_sm(), transitions={'succeeded':'Pre_Grasp', 'preempted':'Reach_Arm', 'aborted':'Reach_Arm'}) smach.StateMachine.add('Pre_Grasp', move_hands_form(hand_pose_name='pre_grasp', hand_side='right'), transitions={'succeeded':'Open_Hand', 'preempted':'Open_Hand', 'aborted':'Open_Hand'}) smach.StateMachine.add('Open_Hand', move_hands_form(hand_pose_name='full_open', hand_side='right'), transitions={'succeeded':'Home_position_end', 'preempted':'Home_position_end', 'aborted':'Home_position_end'}) smach.StateMachine.add('Home_position_end', play_motion_sm("home", 4.0), transitions={'succeeded':'succeeded', 'preempted':'preempted', 'aborted':'aborted'})
def __init__(self): smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'], input_keys=['object_to_grasp'], output_keys=['standard_error']) with self: # Preparation of the Ask Process # Input Data: 'object_to_grasp' self.userdata.tts_wait_before_speaking = 0 self.userdata.tts_lang = 'en_US' smach.StateMachine.add('Prepare_Ask_Person_Object', prepare_ask_person_object(), transitions={'succeeded':'Ask_Person_Object', 'preempted':'Ask_Person_Object', 'aborted':'Ask_Person_Object'}) smach.StateMachine.add('Ask_Person_Object', text_to_say(), transitions={'succeeded':'Reach_Arm', 'preempted':'Reach_Arm', 'aborted':'Reach_Arm'}) # Reach the arm self.userdata.manip_motion_to_play = 'give_object_right' smach.StateMachine.add('Reach_Arm', play_motion_sm(), transitions={'succeeded':'Pre_Grasp', 'preempted':'Reach_Arm', 'aborted':'Reach_Arm'}) smach.StateMachine.add('Pre_Grasp', move_hands_form(hand_pose_name='pre_grasp', hand_side='right'), transitions={'succeeded':'Full_Grasp', 'preempted':'Full_Grasp', 'aborted':'Full_Grasp'}) smach.StateMachine.add('Full_Grasp', move_hands_form(hand_pose_name='grasp', hand_side='right'), transitions={'succeeded':'succeeded', 'preempted':'preempted', 'aborted':'aborted'})
def __init__(self): smach.StateMachine.__init__( self, outcomes=['succeeded', 'aborted', 'preempted'], output_keys=['standard_error']) with self: # Preparation of the Ask Process # Input Data: 'object_to_grasp' self.userdata.tts_wait_before_speaking = 0 self.userdata.tts_lang = 'en_US' smach.StateMachine.add( 'Ask_Person_Object', text_to_say('I am giving you the object you asked'), transitions={ 'succeeded': 'Reach_Arm', 'preempted': 'Reach_Arm', 'aborted': 'Reach_Arm' }) # Reach the arm self.userdata.manip_motion_to_play = 'give_object_right' self.userdata.manip_time_to_play = 2.0 smach.StateMachine.add('Reach_Arm', play_motion_sm(), transitions={ 'succeeded': 'Pre_Grasp', 'preempted': 'Reach_Arm', 'aborted': 'Reach_Arm' }) smach.StateMachine.add('Pre_Grasp', move_hands_form(hand_pose_name='pre_grasp', hand_side='right'), transitions={ 'succeeded': 'Open_Hand', 'preempted': 'Open_Hand', 'aborted': 'Open_Hand' }) smach.StateMachine.add('Open_Hand', move_hands_form(hand_pose_name='full_open', hand_side='right'), transitions={ 'succeeded': 'Home_position_end', 'preempted': 'Home_position_end', 'aborted': 'Home_position_end' }) smach.StateMachine.add('Home_position_end', play_motion_sm("home", 4.0), transitions={ 'succeeded': 'succeeded', 'preempted': 'preempted', 'aborted': 'aborted' })
def __init__(self): smach.StateMachine.__init__( self, outcomes=['succeeded', 'aborted', 'preempted'], input_keys=['object_to_grasp'], output_keys=['standard_error']) with self: # Preparation of the Ask Process # Input Data: 'object_to_grasp' self.userdata.tts_wait_before_speaking = 0 self.userdata.tts_lang = 'en_US' smach.StateMachine.add('Prepare_Ask_Person_Object', prepare_ask_person_object(), transitions={ 'succeeded': 'Ask_Person_Object', 'preempted': 'Ask_Person_Object', 'aborted': 'Ask_Person_Object' }) smach.StateMachine.add('Ask_Person_Object', text_to_say(), transitions={ 'succeeded': 'Reach_Arm', 'preempted': 'Reach_Arm', 'aborted': 'Reach_Arm' }) # Reach the arm self.userdata.manip_motion_to_play = 'give_object_right' smach.StateMachine.add('Reach_Arm', play_motion_sm(), transitions={ 'succeeded': 'Pre_Grasp', 'preempted': 'Reach_Arm', 'aborted': 'Reach_Arm' }) smach.StateMachine.add('Pre_Grasp', move_hands_form(hand_pose_name='pre_grasp', hand_side='right'), transitions={ 'succeeded': 'Full_Grasp', 'preempted': 'Full_Grasp', 'aborted': 'Full_Grasp' }) smach.StateMachine.add('Full_Grasp', move_hands_form(hand_pose_name='grasp', hand_side='right'), transitions={ 'succeeded': 'succeeded', 'preempted': 'preempted', 'aborted': 'aborted' })
def main(): rospy.loginfo('Move_joint_pose_training INIT') sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted']) with sm: smach.StateMachine.add('dummy_state', move_hands_form(hand_pose_name="grasp", hand_side="right"), transitions={'succeeded': 'succeeded','preempted':'preempted', 'aborted':'aborted'}) #smach.StateMachine.add( # 'dummy_state', # move_hands(move_hand_side_out), # transitions={'succeeded': 'succeeded','preempted':'preempted', 'aborted':'aborted'}) sm.execute() rospy.spin()
def main(): rospy.loginfo('Move_joint_pose_training INIT') sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted']) with sm: smach.StateMachine.add('dummy_state', move_hands_form(hand_pose_name="grasp", hand_side="right"), transitions={ 'succeeded': 'succeeded', 'preempted': 'preempted', 'aborted': 'aborted' }) #smach.StateMachine.add( # 'dummy_state', # move_hands(move_hand_side_out), # transitions={'succeeded': 'succeeded','preempted':'preempted', 'aborted':'aborted'}) sm.execute() rospy.spin()
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, ['succeeded', 'preempted', 'aborted'], input_keys=['person_location', 'person_location_coord']) with self: self.userdata.emergency_location = [] self.userdata.tts_lang = 'en_US' self.userdata.tts_wait_before_speaking = 0 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'}) # Ask for the object to bring to the person # Output: # - string 'userSaid' # - actiontag[] 'asr_userSaid_tags' # TODO: grammar for the Emergency Situation -- Get_Person_Desired_Object 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':'Say_go_Kitchen', 'aborted':'Ask_Question', 'aborted':'Ask_Question'}) smach.StateMachine.add( 'Say_go_Kitchen', 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('kitchen'), transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'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'}) # sm_conc = smach.Concurrence(outcomes=['succeeded', 'time_out'], # default_outcome='succeeded', # input_keys=['object_to_grasp, time_grasp'], # child_termination_cb = child_term_cb, # outcome_cb = out_cb) # # with sm_conc: # sm_conc.add( # 'Find_and_grab_object', # #Find_and_grab_object(), # DummyStateMachine()) # sm_conc.add( # 'Time_State', # Time_State()) # # smach.StateMachine.add('GRASP_CONCURRENCE', # sm_conc, # transitions={'succeeded':'Prepare_Go_To_Person', # 'time_out':'Grasp_fail_Ask_Person'}) #Find Object + Grab Object SM # smach.StateMachine.add( # 'Find_and_grab_object', # #Find_and_grab_object(), # DummyStateMachine(), # transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'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':'Prepare_Go_To_Person', 'aborted':'Prepare_Go_To_Person', 'preempted':'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', #DummyStateMachine(), #nav_to_poi(), 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 want.'), transitions={'succeeded':'Give_object_arm', 'aborted':'Give_object_arm', 'preempted':'Give_object_arm'}) 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', #DummyStateMachine(), 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', #DummyStateMachine(), 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, ['succeeded', 'preempted', 'aborted'], input_keys=['person_location', 'person_location_coord']) with self: self.userdata.emergency_location = [] self.userdata.tts_lang = 'en_US' self.userdata.tts_wait_before_speaking = 0 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' }) # Ask for the object to bring to the person # Output: # - string 'userSaid' # - actiontag[] 'asr_userSaid_tags' # TODO: grammar for the Emergency Situation -- Get_Person_Desired_Object 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': 'Say_go_Kitchen', 'aborted': 'Ask_Question', 'aborted': 'Ask_Question' }) smach.StateMachine.add( 'Say_go_Kitchen', 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('kitchen'), transitions={ 'succeeded': 'Grasp_fail_Ask_Person', 'aborted': 'Grasp_fail_Ask_Person', 'preempted': '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' }) # sm_conc = smach.Concurrence(outcomes=['succeeded', 'time_out'], # default_outcome='succeeded', # input_keys=['object_to_grasp, time_grasp'], # child_termination_cb = child_term_cb, # outcome_cb = out_cb) # # with sm_conc: # sm_conc.add( # 'Find_and_grab_object', # #Find_and_grab_object(), # DummyStateMachine()) # sm_conc.add( # 'Time_State', # Time_State()) # # smach.StateMachine.add('GRASP_CONCURRENCE', # sm_conc, # transitions={'succeeded':'Prepare_Go_To_Person', # 'time_out':'Grasp_fail_Ask_Person'}) #Find Object + Grab Object SM # smach.StateMachine.add( # 'Find_and_grab_object', # #Find_and_grab_object(), # DummyStateMachine(), # transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'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': 'Prepare_Go_To_Person', 'aborted': 'Prepare_Go_To_Person', 'preempted': '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', #DummyStateMachine(), #nav_to_poi(), 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 want.'), transitions={ 'succeeded': 'Give_object_arm', 'aborted': 'Give_object_arm', 'preempted': 'Give_object_arm' }) 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', #DummyStateMachine(), 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', #DummyStateMachine(), 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, ['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' })