def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
output_keys=['pose_current'])
with self:
# Calculate the actual position
smach.StateMachine.add(
'get_actual_pos',
get_current_robot_pose(),
transitions={'succeeded': 'save_robot_position', 'aborted': 'get_actual_pos', 'preempted': 'succeeded'})
# Save position
smach.StateMachine.add(
'save_robot_position',
save_robot_position(),
transitions={'succeeded': 'sleep_state', 'aborted': 'save_robot_position', 'preempted':'preempted'})
# Sleep
smach.StateMachine.add(
'sleep_state',
Sleeper(2),
transitions={'succeeded': 'check_button', 'aborted': 'check_button', 'preempted':'preempted'})
# Check if the button is pressed
smach.StateMachine.add(
'check_button',
emergency_button(),
transitions= {'succeeded':'succeeded', 'aborted':'get_actual_pos', 'preempted':'preempted'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
output_keys=['person_location', 'person_location_coord'])
with self:
self.userdata.emergency_location = []
# Some dummy TTS stuff
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
smach.StateMachine.add(
'Prepare_Say_Searching',
prepare_tts('Where are you? Give me signals, please.'),
transitions={'succeeded':'Say_Search', 'aborted':'Say_Search', 'preempted':'Say_Search'})
smach.StateMachine.add(
'Say_Search',
text_to_say(),
transitions={'succeeded':'Gesture_Recognition', 'aborted':'Gesture_Recognition', 'preempted':'Gesture_Recognition'})
# Search for a Wave Gesture
# Output_keys: gesture_detected: type Gesture
self.userdata.nav_to_coord = [0, 0, 0]
#Look Down
#Move head right/left
smach.StateMachine.add(
'Gesture_Recognition',
WaveDetection(),
transitions={'succeeded':'Say_Search','aborted':'Gesture_Recognition', 'preempted':'Gesture_Recognition'})
smach.StateMachine.add(
'Say_Search',
text_to_say('Oh! I have found you.'),
transitions={'succeeded':'Prepare_Go_To_Wave', 'aborted':'Prepare_Go_To_Wave', 'preempted':'Say_Search'})
smach.StateMachine.add(
'Prepare_Go_To_Wave',
prepare_go_to_wave(),
transitions={'succeeded':'Say_Go_to_Wave', 'aborted':'Say_Go_to_Wave', 'preempted':'Say_Go_to_Wave'})
smach.StateMachine.add(
'Say_Go_to_Wave',
text_to_say("I'm coming!"),
transitions={'succeeded':'Go_to_Wave', 'aborted':'Gesture_Recognition', 'preempted':'Gesture_Recognition'})
smach.StateMachine.add(
'Go_to_Wave',
nav_to_coord('/base_link'),
transitions={'succeeded':'Register_Position', 'aborted':'Go_to_Wave', 'preempted':'Go_to_Wave'})
smach.StateMachine.add(
'Register_Position',
get_current_robot_pose(),
transitions={'succeeded':'TreatPoseForCoord', 'aborted':'Register_Position', 'preempted':'Register_Position'},
remapping={'current_robot_pose':'person_location'})
smach.StateMachine.add(
'TreatPoseForCoord',
PoseToArray(),
transitions={'succeeded':'succeeded', 'aborted':'Register_Position', 'preempted':'Register_Position'})
def __init__(self):
smach.StateMachine.__init__(self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['grammar_name'])
with self:
self.userdata.tts_wait_before_speaking=0
self.userdata.tts_text=None
self.userdata.tts_lang=None
self.userdata.nav_to_poi_name=None
self.userdata.standard_error='OK'
self.userdata.asr_userSaid=None
self.userdata.asr_userSaid_tags=None
self.userdata.objectName=""
self.userdata.objectOrientation=""
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={'succeeded': 'LISTEN_TO',
'aborted': 'aborted','preempted':'preempted'})
smach.StateMachine.add('LISTEN_TO',
ListenToSM(GRAMMAR_NAME),
transitions={'succeeded': 'PROCES_TAGS',
'aborted': 'CAN_YOU_REPEAT','preempted':'preempted'})
smach.StateMachine.add('CAN_YOU_REPEAT',
acknowledgment(tts_text=SAY_REPEAT,type_movement='no'),
transitions={'succeeded': 'LISTEN_TO',
'aborted': 'LISTEN_TO','preempted':'preempted'})
smach.StateMachine.add('PROCES_TAGS',
proces_Tags(),
transitions={'new_position': 'GET_POSE','finish':'succeeded',
'aborted': 'CAN_YOU_REPEAT','preempted':'preempted'})
smach.StateMachine.add('GET_POSE',
get_current_robot_pose(),
transitions={'succeeded': 'SAVE_POINT',
'aborted': 'GET_POSE',
'preempted':'preempted'})
#maybe the yaw of the robot we will have to change and say at your right you have....
smach.StateMachine.add('SAVE_POINT',
save_point(),
transitions={'succeeded': 'SAY_OK',
'aborted': 'aborted','preempted':'preempted'})
smach.StateMachine.add('SAY_OK',
acknowledgment(tts_text=SAY_OK,type_movement='yes'),
transitions={'succeeded': 'LISTEN_TO',
'aborted': 'LISTEN_TO','preempted':'preempted'})
def main():
rospy.loginfo('Main Get Current Position')
rospy.init_node('get_current_position')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
smach.StateMachine.add(
'dummy_state',
get_current_robot_pose(),
transitions={'succeeded': 'printUserdata','preempted':'preempted', 'aborted':'aborted'})
smach.StateMachine.add('printUserdata', PrintUserdataPose(), transitions={'succeeded': 'succeeded','preempted':'preempted', 'aborted':'aborted'})
sm.execute()
rospy.spin()
def main():
rospy.loginfo('Main Get Current Position')
rospy.init_node('get_current_position')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
smach.StateMachine.add('dummy_state',
get_current_robot_pose(),
transitions={
'succeeded': 'printUserdata',
'preempted': 'preempted',
'aborted': 'aborted'
})
smach.StateMachine.add('printUserdata',
PrintUserdataPose(),
transitions={
'succeeded': 'succeeded',
'preempted': 'preempted',
'aborted': 'aborted'
})
sm.execute()
rospy.spin()
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['distance'],
output_keys=[])
with self:
# Calculate the actual position
smach.StateMachine.add(
'get_actual_pos',
get_current_robot_pose(),
transitions={'succeeded': 'prepare_move_base', 'aborted': 'aborted', 'preempted': 'succeeded'})
# Prepare the new goal
smach.StateMachine.add(
'prepare_move_base',
prepare_move_base(),
transitions={'succeeded': 'go_to_point', 'aborted': 'aborted', 'preempted':'preempted'})
# Go to the new point
smach.StateMachine.add(
'go_to_point',
nav_to_coord(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})
def __init__(self):
smach.StateMachine.__init__(
self, ['succeeded', 'preempted', 'aborted'],
output_keys=['person_location', 'person_location_coord'])
with self:
self.userdata.emergency_location = []
# Some dummy TTS stuff
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
smach.StateMachine.add(
'Prepare_Say_Searching',
prepare_tts('Where are you? Give me signals, please.'),
transitions={
'succeeded': 'Say_Search',
'aborted': 'Say_Search',
'preempted': 'Say_Search'
})
smach.StateMachine.add('Say_Search',
text_to_say(),
transitions={
'succeeded': 'Gesture_Recognition',
'aborted': 'Gesture_Recognition',
'preempted': 'Gesture_Recognition'
})
# Search for a Wave Gesture
# Output_keys: gesture_detected: type Gesture
self.userdata.nav_to_coord = [0, 0, 0]
#Look Down
#Move head right/left
smach.StateMachine.add('Gesture_Recognition',
WaveDetection(),
transitions={
'succeeded': 'Say_Search',
'aborted': 'Gesture_Recognition',
'preempted': 'Gesture_Recognition'
})
smach.StateMachine.add('Say_Search',
text_to_say('Oh! I have found you.'),
transitions={
'succeeded': 'Prepare_Go_To_Wave',
'aborted': 'Prepare_Go_To_Wave',
'preempted': 'Say_Search'
})
smach.StateMachine.add('Prepare_Go_To_Wave',
prepare_go_to_wave(),
transitions={
'succeeded': 'Say_Go_to_Wave',
'aborted': 'Say_Go_to_Wave',
'preempted': 'Say_Go_to_Wave'
})
smach.StateMachine.add('Say_Go_to_Wave',
text_to_say("I'm coming!"),
transitions={
'succeeded': 'Go_to_Wave',
'aborted': 'Gesture_Recognition',
'preempted': 'Gesture_Recognition'
})
smach.StateMachine.add('Go_to_Wave',
nav_to_coord('/base_link'),
transitions={
'succeeded': 'Register_Position',
'aborted': 'Go_to_Wave',
'preempted': 'Go_to_Wave'
})
smach.StateMachine.add(
'Register_Position',
get_current_robot_pose(),
transitions={
'succeeded': 'TreatPoseForCoord',
'aborted': 'Register_Position',
'preempted': 'Register_Position'
},
remapping={'current_robot_pose': 'person_location'})
smach.StateMachine.add('TreatPoseForCoord',
PoseToArray(),
transitions={
'succeeded': 'succeeded',
'aborted': 'Register_Position',
'preempted': 'Register_Position'
})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
output_keys=['person_location', 'person_location_coord', 'poi_location'])
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
smach.StateMachine.add(
'Home_Play',
play_motion_sm('home'),
transitions={'succeeded':'Search_Person_Room_by_Room','aborted':'Search_Person_Room_by_Room'})
smach.StateMachine.add(
'Search_Person_Room_by_Room',
Search_Emergency_Wave_Room_Change(),
transitions={'succeeded':'Say_Search', 'aborted':'Say_No_People_Found', 'preempted':'Say_Search'})
#This is the worst-case scenario: The person could not be found, so we are losing an important amount of points
smach.StateMachine.add(
'Say_No_People_Found',
text_to_say("I could not find any person in an emergency situation, sorry. Can you come to me?"),
transitions={'succeeded':'face_detection', 'aborted':'aborted'})
#If the person is not found, then it will detect the face
smach.StateMachine.add(
'face_detection',
detect_face(),
transitions={'succeeded': 'Register_Position', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'Say_Search',
text_to_say('Let me help you.'),
transitions={'succeeded':'Prepare_Go_To_Wave', 'aborted':'Prepare_Go_To_Wave', 'preempted':'Say_Search'})
smach.StateMachine.add(
'Prepare_Go_To_Wave',
prepare_go_to_wave(),
transitions={'succeeded':'Say_Go_to_Wave', 'aborted':'Say_Go_to_Wave', 'preempted':'Say_Go_to_Wave'})
smach.StateMachine.add(
'Say_Go_to_Wave',
text_to_say("I'm coming!"),
transitions={'succeeded':'Go_to_Wave', 'aborted':'Go_to_Wave', 'preempted':'Go_to_Wave'})
#The frame_id is '/base_link' because the wave gesture is transformed into this frame, and originally was in xtion
smach.StateMachine.add(
'Go_to_Wave',
nav_to_coord('/base_link'),
transitions={'succeeded':'Say_Arrive_to_Wave', 'aborted':'Go_to_Wave', 'preempted':'Go_to_Wave'})
smach.StateMachine.add(
'Say_Arrive_to_Wave',
text_to_say("I have arrived! "),
transitions={'succeeded':'Register_Position', 'aborted':'Register_Position', 'preempted':'Register_Position'})
smach.StateMachine.add(
'Register_Position',
get_current_robot_pose(),
transitions={'succeeded':'TreatPoseForCoord', 'aborted':'Register_Position', 'preempted':'Register_Position'},
remapping={'current_robot_pose':'person_location'})
smach.StateMachine.add(
'TreatPoseForCoord',
PoseToArray(),
transitions={'succeeded':'succeeded', 'aborted':'Register_Position', 'preempted':'Register_Position'})
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.nav_to_poi_name = ''
# Indicate that we are ready
smach.StateMachine.add(
'say_ready_inspection',
text_to_say("I'm ready for Robot Inspection test"),
transitions={
'succeeded': 'enter_start_door',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Cross start door and go to intermediate point
smach.StateMachine.add('enter_start_door',
EnterRoomSM('intermediate'),
transitions={
'succeeded': 'robot_presentation',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Robot presentation: Little talk + wave gesture
STATES = [
text_to_say("Hi everybody! My name is REEM."),
play_motion_sm("wave", 10)
]
STATE_NAMES = ["say_presentation", "salute_wave"]
outcome_map = {
'succeeded': {
"say_presentation": 'succeeded',
"salute_wave": 'succeeded'
}
}
smach.StateMachine.add("robot_presentation",
ConcurrenceRobocup(states=STATES,
state_names=STATE_NAMES,
outcome_map=outcome_map),
transitions={
'succeeded': 'home_position',
'aborted': "aborted"
})
# Home position
smach.StateMachine.add('home_position',
play_motion_sm('home', 10),
transitions={
'succeeded': 'get_actual_pos',
'aborted': 'aborted',
'preempted': 'succeeded'
})
# Calculate the actual position
smach.StateMachine.add('get_actual_pos',
get_current_robot_pose(),
transitions={
'succeeded': 'wait_time',
'aborted': 'aborted',
'preempted': 'succeeded'
})
# Save position
smach.StateMachine.add('save_robot_position',
save_robot_position(),
transitions={
'succeeded': 'wait_time',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Test of robot
smach.StateMachine.add('wait_time',
Sleeper(20),
transitions={
'succeeded': 'end_time_inspection',
'aborted': 'aborted'
})
# Indicate that we are ready
smach.StateMachine.add('end_time_inspection',
text_to_say("Time finished"),
transitions={
'succeeded': 'set_robot_position',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Set position
smach.StateMachine.add('set_robot_position',
set_robot_position(),
transitions={
'succeeded': 'cross_door_out',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Go to the exit door and cross exit door
# If the door is open change EnterRoom for nav_to_poi
smach.StateMachine.add('cross_door_out',
nav_to_poi('exit_door'),
transitions={
'succeeded': 'succeeded',
'aborted': 'cross_door_out',
'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.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'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.nav_to_poi_name=''
self.userdata.tts_time_before_speaking = 0
self.userdata.tts_text = ""
self.userdata.tts_lang = ""
# Indicate that we are ready
smach.StateMachine.add(
'say_ready_inspection',
text_to_say("I'm ready for Robot Inspection test"),
transitions= {'succeeded':'enter_start_door', 'aborted':'enter_start_door', 'preempted':'preempted'})
# Cross start door and go to intermediate point
smach.StateMachine.add(
'enter_start_door',
EnterRoomSM('intermediate'),
transitions={'succeeded': 'robot_presentation', 'aborted': 'aborted',
'preempted': 'preempted'})
# Robot presentation: Little talk + wave gesture
STATES = [text_to_say("Hi everybody! My name is REEM."), play_motion_sm("wave", 10, True)]
STATE_NAMES = ["say_presentation", "salute_wave"]
outcome_map = {'succeeded': {"say_presentation": 'succeeded', "salute_wave": 'succeeded'}}
smach.StateMachine.add(
"robot_presentation",
ConcurrenceRobocup(states=STATES, state_names=STATE_NAMES, outcome_map=outcome_map),
transitions={'succeeded': 'home_position', 'aborted': "robot_presentation"})
# Home position
smach.StateMachine.add(
'home_position',
play_motion_sm(motion='home', skip_planning=True),
transitions={'succeeded': 'get_actual_pos', 'aborted': 'home_position', 'preempted': 'succeeded'})
# Calculate the actual position
smach.StateMachine.add(
'get_actual_pos',
get_current_robot_pose(),
transitions={'succeeded': 'save_robot_position', 'aborted': 'get_actual_pos', 'preempted': 'succeeded'})
# Save position
smach.StateMachine.add(
'save_robot_position',
save_robot_position(),
transitions={'succeeded': 'say_save_position', 'aborted': 'save_robot_position', 'preempted':'preempted'})
# Indicate that we are saving our position
smach.StateMachine.add(
'say_save_position',
text_to_say("You can press the emergency button whenever you want. I will wait for a minute, inspect me please."),
transitions= {'succeeded':'do_inspection', 'aborted':'do_inspection', 'preempted':'preempted'})
# Test of robot
smach.StateMachine.add(
'do_inspection',
DoInspection(),
transitions={'succeeded': 'say_end_time_inspection', 'aborted': 'say_end_time_inspection'})
# Indicate that we are ready
# smach.StateMachine.add(
# 'end_time_inspection',
# text_to_say("The minute is over."),
# transitions= {'succeeded':'say_end_time_inspection', 'aborted':'say_end_time_inspection', 'preempted':'preempted'})
#
# Indicate that we are ready to go
smach.StateMachine.add(
'say_end_time_inspection',
text_to_say("I hope you are happy with the inspection. I'll leave the room."),
transitions= {'succeeded':'set_robot_position', 'aborted':'set_robot_position', 'preempted':'preempted'})
# Set position
smach.StateMachine.add(
'set_robot_position',
set_robot_position(),
transitions={'succeeded': 'cross_door_out', 'aborted': 'set_robot_position',
'preempted': 'preempted'})
# Go to the exit door and cross exit door
# If the door is open change EnterRoom for nav_to_poi
smach.StateMachine.add(
'cross_door_out',
nav_to_poi('exit_door'),
transitions={'succeeded': 'say_exit', 'aborted': 'cross_door_out',
'preempted': 'preempted'})
# Indicate that we are ready to go
smach.StateMachine.add(
'say_exit',
text_to_say("I arrived successfully to the exit location, Robot Inspection Test complete."),
transitions= {'succeeded':'succeeded', 'aborted':'aborted', 'preempted':'preempted'})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
output_keys=[
'person_location',
'person_location_coord', 'poi_location'
])
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
smach.StateMachine.add('Home_Play',
play_motion_sm('home'),
transitions={
'succeeded':
'Search_Person_Room_by_Room',
'aborted': 'Search_Person_Room_by_Room'
})
smach.StateMachine.add('Search_Person_Room_by_Room',
Search_Emergency_Wave_Room_Change(),
transitions={
'succeeded': 'Say_Search',
'aborted': 'Say_No_People_Found',
'preempted': 'Say_Search'
})
#This is the worst-case scenario: The person could not be found, so we are losing an important amount of points
smach.StateMachine.add(
'Say_No_People_Found',
text_to_say(
"I could not find any person in an emergency situation, sorry. Can you come to me?"
),
transitions={
'succeeded': 'face_detection',
'aborted': 'aborted'
})
#If the person is not found, then it will detect the face
smach.StateMachine.add('face_detection',
detect_face(),
transitions={
'succeeded': 'Register_Position',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('Say_Search',
text_to_say('Let me help you.'),
transitions={
'succeeded': 'Prepare_Go_To_Wave',
'aborted': 'Prepare_Go_To_Wave',
'preempted': 'Say_Search'
})
smach.StateMachine.add('Prepare_Go_To_Wave',
prepare_go_to_wave(),
transitions={
'succeeded': 'Say_Go_to_Wave',
'aborted': 'Say_Go_to_Wave',
'preempted': 'Say_Go_to_Wave'
})
smach.StateMachine.add('Say_Go_to_Wave',
text_to_say("I'm coming!"),
transitions={
'succeeded': 'Go_to_Wave',
'aborted': 'Go_to_Wave',
'preempted': 'Go_to_Wave'
})
#The frame_id is '/base_link' because the wave gesture is transformed into this frame, and originally was in xtion
smach.StateMachine.add('Go_to_Wave',
nav_to_coord('/base_link'),
transitions={
'succeeded': 'Say_Arrive_to_Wave',
'aborted': 'Go_to_Wave',
'preempted': 'Go_to_Wave'
})
smach.StateMachine.add('Say_Arrive_to_Wave',
text_to_say("I have arrived! "),
transitions={
'succeeded': 'Register_Position',
'aborted': 'Register_Position',
'preempted': 'Register_Position'
})
smach.StateMachine.add(
'Register_Position',
get_current_robot_pose(),
transitions={
'succeeded': 'TreatPoseForCoord',
'aborted': 'Register_Position',
'preempted': 'Register_Position'
},
remapping={'current_robot_pose': 'person_location'})
smach.StateMachine.add('TreatPoseForCoord',
PoseToArray(),
transitions={
'succeeded': 'succeeded',
'aborted': 'Register_Position',
'preempted': 'Register_Position'
})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.nav_to_poi_name=''
# Indicate that we are ready
smach.StateMachine.add(
'say_ready_inspection',
text_to_say("I'm ready for Robot Inspection test"),
transitions= {'succeeded':'enter_start_door', 'aborted':'aborted', 'preempted':'preempted'})
# Cross start door and go to intermediate point
smach.StateMachine.add(
'enter_start_door',
EnterRoomSM('intermediate'),
transitions={'succeeded': 'robot_presentation', 'aborted': 'aborted',
'preempted': 'preempted'})
# Robot presentation: Little talk + wave gesture
STATES = [text_to_say("Hi everybody! My name is REEM."), play_motion_sm("wave", 10)]
STATE_NAMES = ["say_presentation", "salute_wave"]
outcome_map = {'succeeded': {"say_presentation": 'succeeded', "salute_wave": 'succeeded'}}
smach.StateMachine.add(
"robot_presentation",
ConcurrenceRobocup(states=STATES, state_names=STATE_NAMES, outcome_map=outcome_map),
transitions={'succeeded': 'home_position', 'aborted': "aborted"})
# Home position
smach.StateMachine.add(
'home_position',
play_motion_sm('home', 10),
transitions={'succeeded': 'get_actual_pos', 'aborted': 'aborted', 'preempted': 'succeeded'})
# Calculate the actual position
smach.StateMachine.add(
'get_actual_pos',
get_current_robot_pose(),
transitions={'succeeded': 'wait_time', 'aborted': 'aborted', 'preempted': 'succeeded'})
# Save position
smach.StateMachine.add(
'save_robot_position',
save_robot_position(),
transitions={'succeeded': 'wait_time', 'aborted': 'aborted', 'preempted':'preempted'})
# Test of robot
smach.StateMachine.add(
'wait_time',
Sleeper(20),
transitions={'succeeded': 'end_time_inspection', 'aborted': 'aborted'})
# Indicate that we are ready
smach.StateMachine.add(
'end_time_inspection',
text_to_say("Time finished"),
transitions= {'succeeded':'set_robot_position', 'aborted':'aborted', 'preempted':'preempted'})
# Set position
smach.StateMachine.add(
'set_robot_position',
set_robot_position(),
transitions={'succeeded': 'cross_door_out', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to the exit door and cross exit door
# If the door is open change EnterRoom for nav_to_poi
smach.StateMachine.add(
'cross_door_out',
nav_to_poi('exit_door'),
transitions={'succeeded': 'succeeded', 'aborted': 'cross_door_out',
'preempted': 'preempted'})
