def main():
rospy.init_node('go_find_face_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
# it call the learn state
smach.StateMachine.add('prepare_face_reconize',
go_find_person(time_out=200),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
sis = smach_ros.IntrospectionServer('detect_face_introspection', sm,
'/DFI_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('go_find_face_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
# it call the learn state
smach.StateMachine.add(
'prepare_face_reconize',
go_find_person(time_out=200),
transitions={'succeeded': 'succeeded','aborted' : 'aborted'})
# This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
sis = smach_ros.IntrospectionServer(
'detect_face_introspection', sm, '/DFI_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
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.nav_to_poi_name = ""
self.userdata.faces = ""
self.userdata.name=''
self.userdata.tts_text = None
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_lang = ''
# Listen the first question
self.userdata.grammar_name = GRAMMAR_NAME
# Home position
smach.StateMachine.add(
'home_position_init',
play_motion_sm('home'),
transitions={'succeeded': 'say_what_did_you_say', 'aborted': 'home_position_init', #TODO: Change aborted to try again
'preempted': 'preempted'})
# Enter room
smach.StateMachine.add(
'say_what_did_you_say',
text_to_say("I'm beginning the what did you say test,. I'm going to the place where the referee should be"),
#transitions={'succeeded': 'go_location', 'aborted': 'aborted'})
transitions={'succeeded': 'go_location', 'aborted': 'aborted'})
# Go to the location
smach.StateMachine.add(
'go_location',
nav_to_poi("init_what_say"),
transitions={'succeeded': 'search_face', 'aborted': 'aborted',
'preempted': 'preempted'})
# smach.StateMachine.add(
# 'say_faces',
# text_to_say("Searching for faces"),
# transitions={'succeeded': 'search_face', 'aborted': 'aborted'})
# Look for a face
smach.StateMachine.add(
'search_face',
go_find_person("init_what_say"),
transitions={'succeeded': 'Say_Found_Face', 'aborted': 'ask_for_tc',
'preempted': 'preempted'},
remapping={'face_frame':'face_frame'})
# Go to the person - We assume that find person will return the position for the person
smach.StateMachine.add(
'Say_Found_Face',
text_to_say('Referee! I have found you at last. Now I am going to you, wait for me.'),
transitions={'succeeded': 'get_current_pose_yaw', 'aborted': 'Say_Found_Face',
'preempted': 'preempted'})
smach.StateMachine.add(
'get_current_pose_yaw',
get_current_robot_pose(),
transitions={'succeeded': 'prepare_coord_person', 'aborted': 'ask_for_tc',
'preempted': 'preempted'})
smach.StateMachine.add(
'prepare_coord_person',
prepare_coord_person(),
transitions={'succeeded': 'go_to_person', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'go_to_person',
nav_to_coord_ud(),
transitions={'succeeded': 'say_found', 'aborted': 'Say_person_not_reached',
'preempted': 'preempted'})
smach.StateMachine.add(
'Say_person_not_reached',
text_to_say('I Found you, but cannot reach you, can you come to me please?'),
transitions={'succeeded': 'ActivateASR', 'aborted': 'aborted'})
# Say "I found you!" + Small Talk
smach.StateMachine.add(
'say_found',
text_to_say("I found you! I have arrived to where you are."),
transitions={'succeeded': 'ActivateASR', 'aborted': 'aborted'})
# Ask for TC if we don't find him
smach.StateMachine.add(
'ask_for_tc',
text_to_say("I can't find you. Can you come to me?"),
transitions={'succeeded': 'wait_for_tc', 'aborted': 'aborted'})
# Wait for TC
smach.StateMachine.add(
'wait_for_tc',
detect_face(),
transitions={'succeeded': 'say_found', 'aborted': 'aborted'})
# Question Part ---------------------------------------------------------------------------------------------
# Init the asr service
# Activate the server
smach.StateMachine.add('ActivateASR',
ActivateASR(),
transitions={'succeeded': 'check_loop', 'aborted': 'aborted', 'preempted': 'preempted'})
# loop test - It checks the number of iterations
smach.StateMachine.add(
'check_loop',
checkLoop(),
transitions={'succeeded': 'ask_next_question', 'aborted': 'aborted',
'preempted': 'preempted', 'end':'DeactivateASR'})
# Ask for next question
smach.StateMachine.add(
'ask_next_question',
text_to_say("I'm ready, ask me a question"),
transitions={'succeeded': 'listen_question', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'listen_question',
ReadASR(),
transitions={'succeeded': 'prepear_repeat', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'prepear_repeat',
prepear_repeat(),
transitions={'succeeded': 'repeat_question', 'aborted': 'aborted',
'preempted': 'preempted'})
# Repeat question
smach.StateMachine.add(
'repeat_question',
text_to_say(),
transitions={'succeeded': 'search_answer', 'aborted': 'aborted',
'preempted': 'preempted'})
# Search the answer
smach.StateMachine.add(
'search_answer',
SelectAnswer(),
transitions={'succeeded': 'say_answer', 'aborted': 'say_answer',
'preempted': 'preempted', 'None': 'aborted'})
# Say the answer
smach.StateMachine.add(
'say_answer',
text_to_say(),
transitions={'succeeded': 'check_loop', 'aborted': 'aborted',
'preempted': 'preempted'})
# Deactivate ASR
smach.StateMachine.add('DeactivateASR',
DeactivateASR(),
transitions={'succeeded': 'say_end', 'aborted': 'aborted', 'preempted': 'preempted'})
smach.StateMachine.add(
'say_end',
text_to_say("What did you say test finished"),
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.nav_to_poi_name = ""
self.userdata.faces = ""
self.userdata.name=''
self.userdata.tts_text = None
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_lang = ''
# Listen the first question
self.userdata.grammar_name = GRAMMAR_NAME
# Home position
smach.StateMachine.add(
'home_position_init',
play_motion_sm('home'),
transitions={'succeeded': 'ActivateASR', 'aborted': 'home_position_init', #TODO: Change aborted to try again
'preempted': 'preempted'})
# Enter room
smach.StateMachine.add(
'say_what_did_you_say',
text_to_say("I'm beginning the what did you say test, I'm going to looking for the referee"),
#transitions={'succeeded': 'go_location', 'aborted': 'aborted'})
transitions={'succeeded': 'search_face', 'aborted': 'search_face'})
# # Go to the location
# smach.StateMachine.add(
# 'go_location',
# nav_to_poi("init_what_say"),
# transitions={'succeeded': 'search_face', 'aborted': 'aborted',
# 'preempted': 'preempted'})
# smach.StateMachine.add(
# 'say_faces',
# text_to_say("Searching for faces"),
# transitions={'succeeded': 'search_face', 'aborted': 'aborted'})
# Look for a face
smach.StateMachine.add(
'search_face',
go_find_person(),
transitions={'succeeded': 'Say_Found_Face', 'aborted': 'ask_for_tc',
'preempted': 'preempted'},
remapping={'face_frame':'face_frame'})
# Go to the person - We assume that find person will return the position for the person
smach.StateMachine.add(
'Say_Found_Face',
text_to_say('Referee! I have found you at last. Now I am going to you, wait for me.'),
transitions={'succeeded': 'get_current_pose_yaw', 'aborted': 'Say_Found_Face',
'preempted': 'preempted'})
smach.StateMachine.add(
'get_current_pose_yaw',
get_current_robot_pose(),
transitions={'succeeded': 'prepare_coord_person', 'aborted': 'ask_for_tc',
'preempted': 'preempted'})
smach.StateMachine.add(
'prepare_coord_person',
prepare_coord_person(),
transitions={'succeeded': 'go_to_person', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'go_to_person',
nav_to_coord_ud(),
transitions={'succeeded': 'say_found', 'aborted': 'Say_person_not_reached',
'preempted': 'preempted'})
smach.StateMachine.add(
'Say_person_not_reached',
text_to_say('I Found you, but cannot reach you, can you come to me please?'),
transitions={'succeeded': 'wait_for_tc', 'aborted': 'aborted'})
# Say "I found you!" + Small Talk
smach.StateMachine.add(
'say_found',
text_to_say("I found you! I have arrived to where you are."),
transitions={'succeeded': 'ActivateASR', 'aborted': 'aborted'})
# Ask for TC if we don't find him
smach.StateMachine.add(
'ask_for_tc',
text_to_say("I can't find you. Can you come to me?"),
transitions={'succeeded': 'wait_for_tc', 'aborted': 'aborted'})
# Wait for TC
smach.StateMachine.add(
'wait_for_tc',
detect_face(),
transitions={'succeeded': 'say_found', 'aborted': 'aborted'})
# Question Part ---------------------------------------------------------------------------------------------
# Init the asr service
# Activate the server
smach.StateMachine.add('ActivateASR',
ActivateASR(),
transitions={'succeeded': 'check_loop', 'aborted': 'aborted', 'preempted': 'preempted'})
# loop test - It checks the number of iterations
smach.StateMachine.add(
'check_loop',
checkLoop(),
transitions={'succeeded': 'ask_next_question', 'aborted': 'aborted',
'preempted': 'preempted', 'end':'DeactivateASR'})
# Ask for next question
smach.StateMachine.add(
'ask_next_question',
text_to_say("I'm ready, ask me a question"),
transitions={'succeeded': 'listen_question', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'listen_question',
ReadASR(),
transitions={'succeeded': 'prepear_repeat', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'prepear_repeat',
prepear_repeat(),
transitions={'succeeded': 'repeat_question', 'aborted': 'aborted',
'preempted': 'preempted'})
# Repeat question
smach.StateMachine.add(
'repeat_question',
text_to_say(),
transitions={'succeeded': 'search_answer', 'aborted': 'aborted',
'preempted': 'preempted'})
# Search the answer
smach.StateMachine.add(
'search_answer',
SelectAnswer(),
transitions={'succeeded': 'say_answer', 'aborted': 'say_answer',
'preempted': 'preempted', 'None': 'aborted'})
# Say the answer
smach.StateMachine.add(
'say_answer',
text_to_say(),
transitions={'succeeded': 'check_loop', 'aborted': 'aborted',
'preempted': 'preempted'})
# Deactivate ASR
smach.StateMachine.add('DeactivateASR',
DeactivateASR(),
transitions={'succeeded': 'say_end', 'aborted': 'aborted', 'preempted': 'preempted'})
smach.StateMachine.add(
'say_end',
text_to_say("What did you say test finished"),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})