def main():
rospy.loginfo('Ask Give Object')
rospy.init_node('ask_give_object_grasping_Training')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.object_to_grasp = 'Coke'
smach.StateMachine.add(
'dummy_state',
ask_give_object_grasping(),
transitions={'succeeded': 'succeeded','preempted':'preempted', 'aborted':'aborted'})
sm.execute()
rospy.spin()
def main():
rospy.loginfo('Ask Give Object')
rospy.init_node('ask_give_object_grasping_Training')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.object_to_grasp = 'Coke'
smach.StateMachine.add('dummy_state',
ask_give_object_grasping(),
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,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[
'object_name', 'name_face', 'list_orders', 'loop_iterations'
],
output_keys=['did_pick', 'name_face'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.try_iterations = 1
self.userdata.did_pick = True
# State that prepare the information for pick - Mock recognition
smach.StateMachine.add('object_recognition',
dummy_recognize(),
transitions={
'succeeded': 'select_object',
'aborted': 'select_object',
'preempted': 'preempted'
})
# Recognize and pick object if found
smach.StateMachine.add('recognize_object_and_pick',
RecObjectAndPick(),
transitions={
'succeeded': 'object_detected',
'fail_grasp': 'Grasp_fail_Ask_Person',
'fail_recognize':
'try_again_recognition'
})
# Check which object I picked and prepare the person name
smach.StateMachine.add('object_detected',
object_detected(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# Recognize Fail Part
# We don't recognized the object
smach.StateMachine.add('try_again_recognition',
checkLoop(),
transitions={
'succeeded':
'recognize_object_and_pick',
'aborted': 'recognize_object_and_pick',
'preempted': 'preempted',
'end': 'Grasp_fail_Ask_Person'
})
#Grasp Mock Part
# Check which object I picked and prepare the person name
smach.StateMachine.add('select_object',
select_object(),
transitions={
'succeeded': 'play_motion_grasp',
'aborted': 'aborted'
})
# Home position
smach.StateMachine.add('play_motion_grasp',
play_motion_sm('home'),
transitions={
'succeeded': 'say_grasp_object',
'preempted': 'say_grasp_object',
'aborted': 'play_motion_grasp'
})
# Say grasp object
smach.StateMachine.add(
'say_grasp_object',
text_to_say("I'm going to grasp the object"),
transitions={
'succeeded': 'grasp_object',
'aborted': 'grasp_object'
})
# Grasp the object
smach.StateMachine.add('grasp_object',
pick_object_sm(),
transitions={
'succeeded': 'succeeded',
'aborted': 'Grasp_fail_Ask_Person',
'preempted': 'preempted'
})
# We ask for the object
# Ask for grasp object
smach.StateMachine.add(
'Grasp_fail_Ask_Person',
ask_give_object_grasping(),
remapping={'object_to_grasp': 'object_name'},
transitions={
'succeeded': 'Rest_arm',
'aborted': 'Rest_arm',
'preempted': 'Rest_arm'
})
smach.StateMachine.add('Rest_arm',
play_motion_sm('rest_object_right'),
transitions={
'succeeded': 'change_did_pick',
'aborted': 'change_did_pick',
'preempted': 'change_did_pick'
})
smach.StateMachine.add('change_did_pick',
change_did_pick(),
transitions={
'succeeded': 'succeeded',
'aborted': 'succeeded',
'preempted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(
self, ['succeeded', 'preempted', 'aborted'],
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'
})
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.gesture_name = ''
self.userdata.object_name = ""
self.userdata.manip_time_to_play = 4
# Must we say something to start? "I'm ready" or something
# Must we wait for the spoken order?
smach.StateMachine.add(
'init_cocktail',
text_to_say("Ready for cocktail party"),
transitions={'succeeded': 'wait_for_door', '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("Searching for wave"),
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"),
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': 'aborted'})
# 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 drink?", GRAMMAR_NAME),
transitions={'succeeded': 'process_order', 'aborted': 'ask_order',
'preempted': 'preempted'})
# Process the answer
smach.StateMachine.add(
'process_order',
process_order(),
transitions={'succeeded': 'search_food_order', 'aborted': 'ask_order',
'preempted': 'preempted'})
# Say what he ask
smach.StateMachine.add(
'say_got_it',
text_to_say("I got it!"),
transitions={'succeeded': 'search_food_order', 'aborted': 'ask_order',
'preempted': 'preempted'})
# Search for object information - It says where the object is, go to it and start object recognition
# TODO: Change how to process object - it must go to storage room always
# TODO: Add some messages in search object
smach.StateMachine.add(
'search_food_order',
SearchObjectSM(),
transitions={'succeeded': 'grasp_food_order', 'aborted': 'Grasp_fail_Ask_Person',
'preempted': 'preempted'})
# Say grasp object
smach.StateMachine.add(
'say_grasp_order',
text_to_say("I'm going to grasp the object"),
transitions={'succeeded': 'grasp_food_order', 'aborted': 'Grasp_fail_Ask_Person',
'preempted': 'preempted'})
# Grasp Object
smach.StateMachine.add(
'grasp_food_order',
DummyStateMachine(),
transitions={'succeeded': 'Grasp_fail_Ask_Person', 'aborted': 'aborted',
'preempted': 'preempted'})
# 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':'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'})
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'})
# Leaving the arena
smach.StateMachine.add(
'leaving_arena',
nav_to_poi('leave_arena'),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['object_name', 'name_face', 'list_orders', 'loop_iterations'],
output_keys=['did_pick','name_face'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.try_iterations = 1
self.userdata.did_pick = True
# State that prepare the information for pick - Mock recognition
smach.StateMachine.add(
'object_recognition',
dummy_recognize(),
transitions={'succeeded': 'select_object', 'aborted': 'select_object',
'preempted': 'preempted'})
# Recognize and pick object if found
smach.StateMachine.add(
'recognize_object_and_pick',
RecObjectAndPick(),
transitions={'succeeded': 'object_detected',
'fail_grasp':'Grasp_fail_Ask_Person',
'fail_recognize': 'try_again_recognition'})
# Check which object I picked and prepare the person name
smach.StateMachine.add(
'object_detected',
object_detected(),
transitions={'succeeded': 'succeeded', 'aborted':'aborted'})
# Recognize Fail Part
# We don't recognized the object
smach.StateMachine.add(
'try_again_recognition',
checkLoop(),
transitions={'succeeded': 'recognize_object_and_pick', 'aborted': 'recognize_object_and_pick',
'preempted': 'preempted', 'end':'Grasp_fail_Ask_Person'})
#Grasp Mock Part
# Check which object I picked and prepare the person name
smach.StateMachine.add(
'select_object',
select_object(),
transitions={'succeeded': 'play_motion_grasp', 'aborted':'aborted'})
# Home position
smach.StateMachine.add(
'play_motion_grasp',
play_motion_sm('home'),
transitions={'succeeded': 'say_grasp_object', 'preempted':'say_grasp_object',
'aborted':'play_motion_grasp'})
# Say grasp object
smach.StateMachine.add(
'say_grasp_object',
text_to_say("I'm going to grasp the object"),
transitions={'succeeded': 'grasp_object', 'aborted': 'grasp_object'})
# Grasp the object
smach.StateMachine.add(
'grasp_object',
pick_object_sm(),
transitions={'succeeded': 'succeeded', 'aborted': 'Grasp_fail_Ask_Person',
'preempted': 'preempted'})
# We ask for the object
# Ask for grasp object
smach.StateMachine.add(
'Grasp_fail_Ask_Person',
ask_give_object_grasping(),
remapping={'object_to_grasp':'object_name'},
transitions={'succeeded':'Rest_arm', 'aborted':'Rest_arm', 'preempted':'Rest_arm'})
smach.StateMachine.add(
'Rest_arm',
play_motion_sm('rest_object_right'),
transitions={'succeeded':'change_did_pick', 'aborted':'change_did_pick', 'preempted':'change_did_pick'})
smach.StateMachine.add(
'change_did_pick',
change_did_pick(),
transitions={'succeeded':'succeeded', 'aborted':'succeeded', 'preempted':'preempted'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
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'})
