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'
})
