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 = ""
# Do the Macarena Dance
smach.StateMachine.add(
"do_macarena",
# DummyStateMachine(),
play_motion_sm(motion="macarena"),
transitions={"succeeded": "do_YMCA", "aborted": "aborted", "preempted": "preempted"},
)
# Do the YMCA Dance
smach.StateMachine.add(
"do_YMCA",
play_motion_sm(motion="ymca"),
transitions={"succeeded": "do_robot", "aborted": "aborted", "preempted": "preempted"},
)
# Do the Robot Dance
smach.StateMachine.add(
"do_robot",
play_motion_sm("robot"),
transitions={"succeeded": "succeed", "aborted": "aborted", "preempted": "preempted"},
)
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, poi_name = None):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['point_to_poi_name'],
output_keys=['standard_error'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
print "POINT TO POI IN-SELF"
self.userdata.standard_error=''
self.userdata.point_to_coord_goal=[0.0,0.0,0.0]
smach.StateMachine.add('PrepareData',
prepareData(poi_name),
transitions={'succeeded':'translate_coord', 'aborted':'aborted'})
# We transform the poi to coordenades
smach.StateMachine.add('translate_coord',
translate_coord(),
transitions={'succeeded': 'get_pose', 'aborted': 'aborted', 'preempted': 'preempted'})
# We get current coodenades of robot in userdata 'current_robot_pose', 'current_robot_yaw', 'pose_current'
smach.StateMachine.add('get_pose',
GetPoseSubscribe(),
transitions={'succeeded': 'get_yaw', 'aborted': 'aborted', 'preempted': 'preempted'})
# We get current coodenades of robot in userdata 'current_robot_pose', 'current_robot_yaw', 'pose_current'
smach.StateMachine.add('get_yaw',
calculateYaw(), #output ['desired_angle']
transitions={'succeeded': 'prepareNav', 'aborted': 'aborted', 'preempted': 'preempted'})
# Prepares to turn
smach.StateMachine.add('prepareNav',
prepare_nav_to_coord(),
transitions={'succeeded': 'turn', 'aborted': 'aborted', 'preempted': 'preempted'})
# Turns
smach.StateMachine.add('turn',
nav_to_coord(),
transitions={'succeeded': 'point_to_coord', 'aborted': 'aborted', 'preempted': 'preempted'})
# Point the coordenades
smach.StateMachine.add('point_to_coord',
play_motion_sm('point_forward'),
transitions={'succeeded': 'Say_Pointing_Poi', 'aborted': 'aborted', 'preempted': 'preempted'})
# Say pointing
smach.StateMachine.add('Say_Pointing_Poi',
text_to_say('There, what I am pointing'),
transitions={'succeeded': 'home_position', 'aborted': 'aborted',
'preempted': 'preempted'})
# Return home position
smach.StateMachine.add('home_position',
play_motion_sm('home'),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})
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, ['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 = 1
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(
'play_motion_state',
play_motion_sm('home', skip_planning=True),
transitions={'succeeded': 'init_cocktail',
'preempted':'init_cocktail',
'aborted':'play_motion_state'})
smach.StateMachine.add(
'init_cocktail',
text_to_say("Ready for cocktail party"),
transitions={'succeeded': 'Ask_order', 'aborted': 'Ask_order'})
# We wait for open door and go inside
smach.StateMachine.add(
'wait_for_door',
EnterRoomSM("party_room"),
transitions={'succeeded': 'Ask_order', 'aborted': 'aborted', 'preempted': 'preempted'})
# Ask Order -> Wave + Learn Person + Order
smach.StateMachine.add(
'Ask_order',
AskAllOrders(),
transitions={'succeeded':'execute_order', 'aborted':'aborted'})
# # Go to the storage_room
# smach.StateMachine.add(
# 'go_to_storage',
# nav_to_poi("storage_room"),
# transitions={'succeeded': 'execute_order', 'aborted': 'go_to_storage',
# 'preempted': 'preempted'})
# Execute the order
smach.StateMachine.add(
'execute_order',
ExecuteAllOrders(),
transitions={'succeeded': 'say_leaving_arena', 'aborted': 'aborted',
'preempted': 'preempted'})
# Say leaving the arena
smach.StateMachine.add(
'say_leaving_arena',
text_to_say("I finished the cocktail party, I'm leaving the arena", wait=False),
transitions={'succeeded': 'succeeded', 'aborted': 'succeeded',
'preempted': 'preempted'})
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', 'preempted', 'aborted', 'fail_grasp', 'fail_recognize'],
input_keys=['object_name'],
output_keys=['object_position','object_detected_name'])
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_lang = ''
self.userdata.tts_wait_before_speak = ''
self.userdata.tts_text = ''
# Say start object recognition
smach.StateMachine.add(
'say_start_obj_recognition',
text_to_say("I'm going to start the Object recognition process.", wait=False),
transitions={'succeeded': 'object_recognition', 'aborted': 'object_recognition'})
# Do object_recognition
smach.StateMachine.add(
'object_recognition',
recognize_object(),
transitions={'succeeded': 'process_object_recognition', 'aborted': 'fail_recognize',
'preempted': 'preempted'})
# Process the objects recognized
smach.StateMachine.add(
'process_object_recognition',
process_pick_location(),
transitions={'succeeded': 'say_grasp_object', 'aborted': 'fail_recognize',
'preempted': 'preempted'})
# Say grasp object
smach.StateMachine.add(
'say_grasp_object',
text_to_say("I'm going to grasp the object", wait=False),
transitions={'succeeded': 'grasp_object', 'aborted': 'aborted'})
# Grasp the object
smach.StateMachine.add(
'grasp_object',
pick_object_sm(),
transitions={'succeeded': 'succeeded', 'aborted': 'home_position',
'preempted': 'preempted'})
# Home position
smach.StateMachine.add(
'home_position',
play_motion_sm('home',skip_planning=True),
transitions={'succeeded': 'fail_grasp', 'aborted': 'home_position',
'preempted': 'preempted'})
def __init__(self, type_movement="yes", tts_text=''):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['tts_text', 'type_movment'],
output_keys=['standard_error'])
self.type_movement = type_movement
self.tts_text = tts_text
with self:
self.userdata.tts_text = None
self.userdata.type_movment = None
self.userdata.tts_lang = None
self.userdata.tts_wait_before_speaking = 0
self.userdata.standard_error = 'OK'
self.userdata.manip_time_to_play = 30
self.userdata.skip_planning = False
smach.StateMachine.add('INIT_VAR',
init_var(self.type_movement, self.tts_text),
transitions={
'succeeded': 'PUT_MOVMENT',
'aborted': 'aborted',
'preempted': 'preempted'
})
sm = smach.Concurrence(
outcomes=['succeeded', 'preempted', 'aborted'],
default_outcome='succeeded',
input_keys=[
'tts_text', 'manip_motion_to_play', 'manip_time_to_play',
'tts_wait_before_speaking', 'tts_lang', 'skip_planning'
])
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_text = None
self.userdata.tts_lang = None
with sm:
sm.add('SAY', text_to_say())
sm.add('MOVE', play_motion_sm(skip_planning=False))
smach.StateMachine.add('PUT_MOVMENT',
sm,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
def main():
rospy.loginfo('Play Motion Training Node')
rospy.init_node('play_motion_training_node')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.manip_motion_to_play = 'arms_t'
sm.userdata.manip_time_to_play = 8.0
smach.StateMachine.add(
'play_motion_state',
play_motion_sm(),
transitions={'succeeded': 'succeeded','preempted':'preempted', 'aborted':'aborted'})
sm.execute()
rospy.spin()
def __init__(self,type_movement="yes",tts_text=''):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['tts_text','type_movment'],
output_keys=['standard_error'])
self.type_movement=type_movement
self.tts_text=tts_text
with self:
self.userdata.tts_text=None
self.userdata.type_movment = None
self.userdata.tts_lang=None
self.userdata.tts_wait_before_speaking=0
self.userdata.standard_error='OK'
self.userdata.manip_time_to_play=30
self.userdata.skip_planning=False
smach.StateMachine.add(
'INIT_VAR',
init_var(self.type_movement,
self.tts_text),
transitions={'succeeded': 'PUT_MOVMENT', 'aborted': 'aborted',
'preempted': 'preempted'})
sm=smach.Concurrence(outcomes=['succeeded', 'preempted','aborted'],
default_outcome='succeeded',input_keys=['tts_text',
'manip_motion_to_play',
'manip_time_to_play',
'tts_wait_before_speaking',
'tts_lang','skip_planning'])
self.userdata.tts_wait_before_speaking=0
self.userdata.tts_text=None
self.userdata.tts_lang=None
with sm:
sm.add('SAY',
text_to_say())
sm.add('MOVE',
play_motion_sm(skip_planning=False))
smach.StateMachine.add('PUT_MOVMENT', sm,
transitions={'succeeded':'succeeded',
'aborted':'aborted','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.nav_to_poi_name = ''
# Do the Macarena Dance
smach.StateMachine.add(
'do_macarena',
#DummyStateMachine(),
play_motion_sm(motion='macarena'),
transitions={
'succeeded': 'do_YMCA',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Do the YMCA Dance
smach.StateMachine.add('do_YMCA',
play_motion_sm(motion='ymca'),
transitions={
'succeeded': 'do_robot',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Do the Robot Dance
smach.StateMachine.add('do_robot',
play_motion_sm('robot'),
transitions={
'succeeded': 'succeed',
'aborted': 'aborted',
'preempted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error'])
with self:
# Home position
smach.StateMachine.add(
'home_position_init',
play_motion_sm('home'),
transitions={
'succeeded': 'prepare_Avoid',
'aborted':
'home_position_init', #TODO: Change aborted to try again
'preempted': 'preempted'
})
# We prepare the information to go to the init door
smach.StateMachine.add('prepare_Avoid',
prepare_Avoid(),
transitions={
'succeeded': 'say_go_to_poi',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Announce going to a place
smach.StateMachine.add(
'say_go_to_poi',
text_to_say(text="I'm going to the Avoid That location."),
transitions={'succeeded': 'go_to_poi'})
# Go to the POI
smach.StateMachine.add('go_to_poi',
nav_to_poi(),
transitions={
'succeeded': 'say_get_to_poi',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Announce arriving to a place
smach.StateMachine.add(
'say_get_to_poi',
text_to_say(
text="I've arrived to the final destination for Avoid That."
),
transitions={'succeeded': 'succeeded'})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
output_keys=[],
input_keys=[])
with self:
#maybe i will have to learn again
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={
'succeeded': 'look_wave',
'aborted': 'aborted',
'preempted': 'preempted'
})
#maybe i will have to learn again
smach.StateMachine.add('look_wave',
WaveDetection(),
transitions={
'succeeded': 'say_hello',
'aborted': 'look_wave',
'preempted': 'preempted'
})
smach.StateMachine.add('say_hello',
text_to_say(Say, wait=False),
transitions={
'succeeded': 'wait_state',
'aborted': 'wait_state'
})
smach.StateMachine.add('play_motion_state',
play_motion_sm('wave', skip_planning=True),
transitions={
'succeeded': 'wait_state',
'preempted': 'wait_state',
'aborted': 'wait_state'
})
smach.StateMachine.add('wait_state',
waitstate(),
transitions={
'succeeded': 'look_wave',
'aborted': 'look_wave',
'preempted': 'look_wave'
})
def main():
rospy.loginfo('Play Motion Training Node')
rospy.init_node('play_motion_training_node')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.manip_motion_to_play = 'arms_t'
sm.userdata.manip_time_to_play = 8.0
smach.StateMachine.add('play_motion_state',
play_motion_sm(),
transitions={
'succeeded': 'succeeded',
'preempted': 'preempted',
'aborted': 'aborted'
})
sm.execute()
rospy.spin()
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['list_orders'],
output_keys=['list_orders'])
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 = 1
self.userdata.object_name = []
self.userdata.list_orders = [['hannah', 'red bull'], ['rebecca', 'water'], ['andrew', 'Cocacola']]
# Go to the storage_room
smach.StateMachine.add(
'go_to_storage',
nav_to_poi("storage_room"),
transitions={'succeeded': 'prepare_recognize', 'aborted': 'prepare_recognize',
'preempted': 'preempted'})
# Prepare next order
smach.StateMachine.add(
'prepare_recognize',
prepare_recognize(),
transitions={'succeeded': 'execute_order', 'aborted': 'aborted',
'preempted': 'preempted'})
# Execute Order
smach.StateMachine.add(
'execute_order',
ExecuteOrder(),
transitions={'succeeded': 'check_loop', 'aborted': 'aborted'})
# End of loop?
smach.StateMachine.add(
'check_loop',
checkLoop(),
transitions={'succeeded': 'play_motion_state', 'aborted': 'aborted',
'preempted': 'preempted', 'end':'succeeded'})
smach.StateMachine.add(
'play_motion_state',
play_motion_sm('home', skip_planning=True),
transitions={'succeeded': 'go_to_storage',
'preempted':'go_to_storage',
'aborted':'play_motion_state'})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
output_keys=[],
input_keys=[])
with self:
#maybe i will have to learn again
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={'succeeded': 'look_wave',
'aborted': 'aborted','preempted':'preempted'})
#maybe i will have to learn again
smach.StateMachine.add('look_wave',
WaveDetection(),
transitions={'succeeded': 'say_hello',
'aborted': 'look_wave','preempted':'preempted'})
smach.StateMachine.add(
'say_hello',
text_to_say(Say,wait=False),
transitions={'succeeded': 'wait_state', 'aborted': 'wait_state'})
smach.StateMachine.add(
'play_motion_state',
play_motion_sm('wave', skip_planning=True),
transitions={'succeeded': 'wait_state',
'preempted':'wait_state',
'aborted':'wait_state'})
smach.StateMachine.add(
'wait_state',
waitstate(),
transitions={'succeeded': 'look_wave', 'aborted': 'look_wave',
'preempted': 'look_wave'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'], output_keys=['standard_error'])
with self:
# Home position
smach.StateMachine.add(
'home_position_init',
play_motion_sm('home'),
transitions={'succeeded': 'prepare_Avoid', 'aborted': 'home_position_init', #TODO: Change aborted to try again
'preempted': 'preempted'})
# We prepare the information to go to the init door
smach.StateMachine.add(
'prepare_Avoid',
prepare_Avoid(),
transitions={'succeeded': 'say_go_to_poi', 'aborted': 'aborted',
'preempted': 'preempted'})
# Announce going to a place
smach.StateMachine.add(
'say_go_to_poi',
text_to_say(text="I'm going to the Avoid That area."),
transitions={'succeeded': 'go_to_poi'})
# Go to the POI
smach.StateMachine.add(
'go_to_poi',
nav_to_poi(),
transitions={'succeeded': 'say_get_to_poi', 'aborted': 'aborted',
'preempted': 'preempted'})
# Announce arriving to a place
smach.StateMachine.add(
'say_get_to_poi',
text_to_say(text="I've arrived to the destination for Avoid That. The test Avoid That has finished successfully. Thank you."),
transitions={'succeeded': 'succeeded'})
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_lang = ''
self.userdata.tts_wait_before_speak = ''
self.userdata.tts_text = ''
# Say start Pick and Place
smach.StateMachine.add(
'say_start_pick_place',
text_to_say("I'm going to start Pick and Place test"),
transitions={'succeeded': 'say_start_obj_recognition', 'aborted': 'say_start_obj_recognition'})
# Say Going to location
# smach.StateMachine.add(
# 'say_go_location',
# text_to_say("I'm going to the location where I have to recognize some objects",wait=False),
# transitions={'succeeded': 'prepare_location', 'aborted': 'prepare_location'})
#
# # Prepare the poi for nav_to_poi
# smach.StateMachine.add(
# 'prepare_location',
# prepare_location(),
# transitions={'succeeded': 'go_location', 'aborted': 'prepare_location',
# 'preempted': 'preempted'})
#
# # Go to the location
# smach.StateMachine.add(
# 'go_location',
# nav_to_poi(),
# transitions={'succeeded': 'say_start_obj_recognition', 'aborted': 'say_go_location',
# 'preempted': 'preempted'})
# 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_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': 'say_go_second_location', 'aborted': 'play_motion_grasp',
'preempted': 'preempted'})
# 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 go to second location
smach.StateMachine.add(
'say_go_second_location',
text_to_say("I'm going to the location where I should release the object"),
transitions={'succeeded': 'go_second_location', 'aborted': 'go_second_location'})
# Go the location - We need to go to the place to object category, so we assume that the
# object recognition will init the poi to the object must to go
smach.StateMachine.add(
'go_second_location',
nav_to_poi(),
transitions={'succeeded': 'say_release_obj', 'aborted': 'say_go_second_location',
'preempted': 'preempted'})
# Say release object
smach.StateMachine.add(
'say_release_obj',
text_to_say("I'm going to release the object"),
transitions={'succeeded': 'pregrasp_state', 'aborted': 'pregrasp_state'})
# Pre-grasp position
smach.StateMachine.add(
'pregrasp_state',
play_motion_sm('pre_grasp', skip_planning=True),
transitions={'succeeded': 'play_motion_state', 'preempted':'play_motion_state',
'aborted':'pregrasp_state'})
# Release the object
smach.StateMachine.add(
'release_object',
place_object_sm(),
transitions={'succeeded': 'play_motion_state', 'aborted': 'say_release_obj',
'preempted': 'preempted'})
# Home position
smach.StateMachine.add(
'play_motion_state',
play_motion_sm('home'),
transitions={'succeeded': 'say_end_pick_place',
'preempted':'say_end_pick_place',
'aborted':'say_end_pick_place'})
# Say end Pick and Place
smach.StateMachine.add(
'say_end_pick_place',
text_to_say("I finished the Pick and Place test"),
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!
# Robot presentation: Little talk + wave gesture
STATES = [text_to_say("Hi everybody! My name is REEM."), play_motion_sm("wave")]
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": "object_recognition", "aborted": "robot_presentation"},
)
# Do object_recognition
smach.StateMachine.add(
"object_recognition",
dummy_recognize(),
transitions={"succeeded": "say_grasp_object", "aborted": "say_grasp_object", "preempted": "preempted"},
)
# Say grasp object
smach.StateMachine.add(
"say_grasp_object",
text_to_say("I am hungry. I am going to pick the noodles"),
transitions={"succeeded": "grasp_object", "aborted": "grasp_object"},
)
# Grasp the object
smach.StateMachine.add(
"grasp_object",
pick_object_sm(),
transitions={"succeeded": "say_why_object", "aborted": "play_motion_state", "preempted": "preempted"},
)
# Say release object
smach.StateMachine.add(
"say_why_object",
text_to_say(
"I am a robot, I can't eat this. I am so sad! I am going to leave the noodles in the table",
wait=False,
),
transitions={"succeeded": "release_object", "aborted": "release_object"},
)
# Release the object
smach.StateMachine.add(
"release_object",
place_object_sm(),
transitions={"succeeded": "robot_finish", "aborted": "aborted", "preempted": "preempted"},
)
# Say hard job + bow
STATES = [
text_to_say("Uff, this was a hard job. Thank you very much for your attention"),
play_motion_sm("bow"),
]
STATE_NAMES = ["say_hard_job", "motion_bow"]
outcome_map = {"succeeded": {"say_hard_job": "succeeded", "motion_bow": "succeeded"}}
smach.StateMachine.add(
"robot_finish",
ConcurrenceRobocup(states=STATES, state_names=STATE_NAMES, outcome_map=outcome_map),
transitions={"succeeded": "say_rest", "aborted": "say_rest"},
)
# Say go rest
smach.StateMachine.add(
"say_rest",
text_to_say("I am going to rest for a few seconds", wait=False),
transitions={"succeeded": "sleep_robot", "aborted": "sleep_robot"},
)
# Sleep
smach.StateMachine.add(
"sleep_robot",
Sleeper(15),
transitions={
"succeeded": "robot_presentation",
"preempted": "robot_presentation",
"aborted": "robot_presentation",
},
)
# Home position
smach.StateMachine.add(
"play_motion_state",
play_motion_sm("home"),
transitions={
"succeeded": "say_grasp_object",
"preempted": "say_grasp_object",
"aborted": "say_grasp_object",
},
)
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
output_keys=[],
input_keys=[])
with self:
#maybe i will have to learn again
smach.StateMachine.add('INIT_VAR',
waitstate(),
transitions={'succeeded': 'play_motion_state',
'aborted': 'aborted','preempted':'preempted'})
smach.StateMachine.add(
'play_motion_state',
play_motion_sm('home', skip_planning=False),
transitions={'succeeded': 'WAIT_0',
'preempted':'WAIT_0',
'aborted':'WAIT_0'})
smach.StateMachine.add('WAIT_0',
waitstate(),
transitions={'succeeded': 'say_0',
'aborted': 'say_0','preempted':'preempted'})
smach.StateMachine.add(
'say_0',
text_to_say("hello my name is reem, i'm really happy to be here and show you my movements. I come from Barcelona. I was created by pal robotics. I came here with my team reem at la salle.",wait=False),
transitions={'succeeded': 'play_motion_state_0', 'aborted': 'play_motion_state_0', 'preempted': 'play_motion_state_0'})
smach.StateMachine.add(
'play_motion_state_0',
play_motion_sm('wave', skip_planning=False),
transitions={'succeeded': 'WAIT_1',
'preempted':'WAIT_1',
'aborted':'WAIT_1'})
smach.StateMachine.add('WAIT_1',
waitstate(),
transitions={'succeeded': 'play_motion_state_1',
'aborted': 'play_motion_state_1','preempted':'preempted'})
smach.StateMachine.add(
'play_motion_state_1',
play_motion_sm('arms_t', skip_planning=False),
transitions={'succeeded': 'WAIT_2',
'preempted':'WAIT_2',
'aborted':'WAIT_2'})
smach.StateMachine.add('WAIT_2',
waitstate(),
transitions={'succeeded': 'play_motion_state_2',
'aborted': 'play_motion_state_2','preempted':'preempted'})
smach.StateMachine.add(
'play_motion_state_2',
play_motion_sm('wave', skip_planning=False),
transitions={'succeeded': 'WAIT_3',
'preempted':'WAIT_3',
'aborted':'WAIT_3'})
smach.StateMachine.add('WAIT_3',
waitstate(),
transitions={'succeeded': 'play_motion_state_3',
'aborted': 'play_motion_state_3','preempted':'preempted'})
smach.StateMachine.add(
'play_motion_state_3',
play_motion_sm('brochure_tray', skip_planning=False),
transitions={'succeeded': 'WAIT_4',
'preempted':'WAIT_4',
'aborted':'WAIT_4'})
smach.StateMachine.add('WAIT_4',
waitstate(),
transitions={'succeeded': 'play_motion_state_4',
'aborted': 'play_motion_state_4','preempted':'preempted'})
smach.StateMachine.add(
'play_motion_state_4',
play_motion_sm('carry_basquet', skip_planning=False),
transitions={'succeeded': 'WAIT_5',
'preempted':'WAIT_5',
'aborted':'WAIT_5'})
smach.StateMachine.add('WAIT_5',
waitstate(),
transitions={'succeeded': 'play_motion_state_5',
'aborted': 'play_motion_state_5','preempted':'preempted'})
smach.StateMachine.add(
'play_motion_state_5',
play_motion_sm('shake_right', skip_planning=False),
transitions={'succeeded': 'WAIT_6',
'preempted':'WAIT_6',
'aborted':'WAIT_6'})
smach.StateMachine.add('WAIT_6',
waitstate(),
transitions={'succeeded': 'play_motion_state_6',
'aborted': 'play_motion_state_6','preempted':'preempted'})
smach.StateMachine.add(
'play_motion_state_6',
play_motion_sm('open_arms', skip_planning=False),
transitions={'succeeded': 'WAIT_0',
'preempted':'WAIT_0',
'aborted':'WAIT_0'})
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=''
self.userdata.manip_motion_to_play = ''
smach.StateMachine.add(
'play_motion_state',
play_motion_sm('home'),
transitions={'succeeded': 'say_start_basic_functionalities',
'preempted':'say_start_basic_functionalities',
'aborted':'say_start_basic_functionalities'})
# Say Start basic Functionalities
smach.StateMachine.add(
'say_start_basic_functionalities',
text_to_say("I'm ready to start Basic Functionalities"),
transitions={'succeeded': 'say_going_pick_place', 'aborted': 'say_going_pick_place'})
# Say Go Pick and Place
smach.StateMachine.add(
'say_going_pick_place',
text_to_say("I'm going to the Pick and Place location"),
transitions={'succeeded': 'go_pick_and_place', 'aborted': 'go_pick_and_place'})
# Go to pick and place
smach.StateMachine.add(
'go_pick_and_place',
Go_Poi_Listen_Word('init_pick_and_place', SENTENCE_STOP),
#Go_Poi_Listen_Word('point_room_one', SENTENCE_STOP),
#transitions={'succeeded': 'say_going_avoid', 'aborted': 'confirm_stop_pick_place',
#'preempted': 'preempted'})
transitions={'succeeded':'pick_timer', 'aborted': 'confirm_stop_pick_place',
'preempted': 'preempted'})
# The robot wait for "move" to move to the poi
smach.StateMachine.add(
'confirm_stop_pick_place',
acknowledgment("yes", "Okey, I stop"),
transitions={'succeeded': 'wait_pick_and_place', 'aborted': 'wait_pick_and_place',
'preempted': 'preempted'})
smach.StateMachine.add(
'wait_pick_and_place',
ListenWordSM_Concurrent(SENTENCE_GO),
transitions={'succeeded': 'confirm_move_pick_place', 'aborted': 'wait_pick_and_place',
'preempted': 'preempted'})
smach.StateMachine.add(
'confirm_move_pick_place',
acknowledgment("yes", "Okey, I move"),
transitions={'succeeded': 'say_going_pick_place', 'aborted': 'say_going_pick_place',
'preempted': 'preempted'})
# Do pick and place + TimeOut
STATES = [PickPlaceSM(), TimeOut(180)]
STATE_NAMES = ["do_pick_and_place", "time_pick"]
outcome_map = {'succeeded': {"do_pick_and_place": 'succeeded'}, 'aborted': {"time_pick": 'succeeded'}}
smach.StateMachine.add(
"pick_timer",
ConcurrenceRobocup(states=STATES, state_names=STATE_NAMES, outcome_map=outcome_map),
transitions={'succeeded': 'say_going_avoid', 'aborted': "timeout_pick_and_place"})
# Say TimeOut
smach.StateMachine.add(
'timeout_pick_and_place',
text_to_say("The time for Pick and Place is finish"),
transitions={'succeeded': 'say_going_avoid', 'aborted': 'say_going_avoid',
'preempted': 'preempted'})
# smach.StateMachine.add(
# 'do_pick_and_place',
# PickPlaceSM(),
# transitions={'succeeded': 'say_going_avoid', 'aborted': 'say_going_avoid',
# 'preempted': 'preempted'})
# Say Go Avoid that
smach.StateMachine.add(
'say_going_avoid',
text_to_say("I'm going to the Avoid that Area"),
transitions={'succeeded': 'go_avoid_that', 'aborted': 'go_avoid_that'})
# Go to avoid that
smach.StateMachine.add(
'go_avoid_that',
Go_Poi_Listen_Word('init_avoid_that', SENTENCE_STOP),
#Go_Poi_Listen_Word('point_room_two', SENTENCE_STOP),
#transitions={'succeeded': 'say_going_what_say', 'aborted': 'confirm_stop_avoid_that',
#'preempted': 'preempted'})
transitions={'succeeded': 'avoid_timer', 'aborted': 'confirm_stop_avoid_that',
'preempted': 'preempted'})
# The robot wait for "move" to move to the poi
smach.StateMachine.add(
'confirm_stop_avoid_that',
acknowledgment("yes", "Okey, I stop"),
transitions={'succeeded': 'wait_avoid_that', 'aborted': 'wait_avoid_that',
'preempted': 'preempted'})
smach.StateMachine.add(
'wait_avoid_that',
ListenWordSM_Concurrent(SENTENCE_GO),
transitions={'succeeded': 'confirm_move_avoid_that', 'aborted': 'wait_avoid_that',
'preempted': 'preempted'})
smach.StateMachine.add(
'confirm_move_avoid_that',
acknowledgment("yes", "Okey, I move"),
transitions={'succeeded': 'say_going_avoid', 'aborted': 'say_going_avoid',
'preempted': 'preempted'})
# Do avoid that + TimeOut
STATES = [Avoid_That(), TimeOut(180)]
STATE_NAMES = ["do_avoid_that", "time_avoid"]
outcome_map = {'succeeded': {"do_avoid_that": 'succeeded'}, 'aborted': {"time_avoid": 'succeeded'}}
smach.StateMachine.add(
"avoid_timer",
ConcurrenceRobocup(states=STATES, state_names=STATE_NAMES, outcome_map=outcome_map),
transitions={'succeeded': 'say_going_what_say', 'aborted': "timeout_avoid_that"})
# Say TimeOut
smach.StateMachine.add(
'timeout_avoid_that',
text_to_say("The time for Avoid That is finish"),
transitions={'succeeded': 'say_going_what_say', 'aborted': 'say_going_what_say',
'preempted': 'preempted'})
# Do avoid that
# smach.StateMachine.add(
# 'do_avoid_that',
# Avoid_That(),
# transitions={'succeeded': 'say_going_what_say', 'aborted': 'say_going_what_say',
# 'preempted': 'preempted'})
# Say Go What did you say
smach.StateMachine.add(
'say_going_what_say',
text_to_say("I'm going to the What did you say Location"),
transitions={'succeeded': 'go_what_did_you_say', 'aborted': 'go_what_did_you_say'})
# Go to what did you say
smach.StateMachine.add(
'go_what_did_you_say',
Go_Poi_Listen_Word('init_what_say', SENTENCE_STOP),
#Go_Poi_Listen_Word('point_room_three', SENTENCE_STOP),
#transitions={'succeeded': 'say_finish_basic_functionalities', 'aborted': 'confirm_stop_what_say',
#'preempted': 'preempted'})
transitions={'succeeded': 'what_say_timer', 'aborted': 'confirm_stop_what_say',
'preempted': 'preempted'})
# The robot wait for "move" to move to the poi
smach.StateMachine.add(
'confirm_stop_what_say',
acknowledgment("yes", "Okey, I stop"),
transitions={'succeeded': 'wait_what_did_you_say', 'aborted': 'wait_what_did_you_say',
'preempted': 'preempted'})
smach.StateMachine.add(
'wait_what_did_you_say',
ListenWordSM_Concurrent(SENTENCE_GO),
transitions={'succeeded': 'confirm_move_what_say', 'aborted': 'wait_what_did_you_say',
'preempted': 'preempted'})
smach.StateMachine.add(
'confirm_move_what_say',
acknowledgment("yes", "Okey, I move"),
transitions={'succeeded': 'say_going_what_say', 'aborted': 'say_going_what_say',
'preempted': 'preempted'})
# Do what did you say + TimeOut
STATES = [WhatSaySM(), TimeOut(180)]
STATE_NAMES = ["do_what_did_you_say", "time_what_say"]
outcome_map = {'succeeded': {"do_what_did_you_say": 'succeeded'}, 'aborted': {"time_what_say": 'succeeded'}}
smach.StateMachine.add(
"what_say_timer",
ConcurrenceRobocup(states=STATES, state_names=STATE_NAMES, outcome_map=outcome_map),
transitions={'succeeded': 'say_going_what_say', 'aborted': "timeout_what_say"})
# Say TimeOut
smach.StateMachine.add(
'timeout_what_say',
text_to_say("The time for What did you say is finish"),
transitions={'succeeded': 'say_finish_basic_functionalities', 'aborted': 'say_finish_basic_functionalities',
'preempted': 'preempted'})
# Do what did you say
# smach.StateMachine.add(
# 'do_what_did_you_say',
# WhatSaySM(),
# transitions={'succeeded': 'say_finish_basic_functionalities', 'aborted': 'say_finish_basic_functionalities',
# 'preempted': 'preempted'})
# Say Finish basic Functionalities
smach.StateMachine.add(
'say_finish_basic_functionalities',
text_to_say("I finished Basic Functionalities"),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
def __init__(self):
sm = smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'])
with self:
smach.StateMachine.add(
'Say_Init',
text_to_say(
"Initializing and Preparing Emergency Test. Let's go!"),
transitions={'succeeded': 'Arms_Home'})
# Put the arms at the 'Home' position, in order improve the security
# We prepare the information to go to the init door
self.userdata.manip_motion_to_play = 'home'
self.userdata.manip_time_to_play = 4.0
smach.StateMachine.add('Arms_Home',
play_motion_sm(),
transitions={
'succeeded': 'Prepare_TTS_1',
'aborted': 'Prepare_TTS_1',
'preempted': 'Prepare_TTS_1'
})
#Prepare the data for the speech: language and time to wait
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_lang = 'en_US'
smach.StateMachine.add(
'Prepare_TTS_1',
prepare_tts(
"Now I am going to enter the Room to help people in need."
),
transitions={'succeeded': 'Say_Ready'})
smach.StateMachine.add('Say_Ready',
text_to_say(),
transitions={
'succeeded': 'Enter_Room_Arena',
'aborted': 'Enter_Room_Arena',
'preempted': 'Enter_Room_Arena'
})
# Pass the entry room
# TODO: Define the poi for the output of the room
# Pre: The robot should be in front of the Arena door (maybe we should change this, depending on the conditions)
#smach.StateMachine.add(
# 'Prepare_Door_Out_Arena',
# prepare_poi_emergency('entry_door_exit'),
# transitions={'succeeded':'Enter_Room_Arena', 'aborted':'Enter_Room_Arena', 'preempted':'Enter_Room_Arena'})
smach.StateMachine.add('Enter_Room_Arena',
EnterRoomSM('entry_door_exit'),
transitions={
'succeeded': 'Say_Enter_Successful',
'aborted': 'Prepare_TTS_1',
'preempted': 'Prepare_Poi_Emergency_1'
})
smach.StateMachine.add(
'Say_Enter_Successful',
text_to_say(
'Successfully entering the room. Now I am going to the Emergency Room.'
),
transitions={
'succeeded': 'Prepare_Poi_Emergency_1',
'aborted': 'Prepare_Poi_Emergency_1',
'preempted': 'Prepare_Poi_Emergency_1'
})
# Go to the emergency location
# TODO: Define the name of the room to enter (defined by the OC)
# If Aborted (not supposed to), retry?
smach.StateMachine.add(
'Prepare_Poi_Emergency_1',
prepare_poi_emergency('emergency_room'),
transitions={'succeeded': 'Go_to_emergency_room'})
smach.StateMachine.add('Go_to_emergency_room',
nav_to_poi(),
transitions={
'succeeded': 'Search_Person',
'aborted': 'Go_to_emergency_room',
'preempted': 'Go_to_emergency_room'
})
# Userdata output keys:
# - person_location: PoseStamped/-->Pose<-- (?)
# Another state will be needed (maybe) to remap
# No need of face_recognition
# What if person not found? Re-search?
smach.StateMachine.add('Search_Person',
Search_People_Emergency(),
transitions={
'succeeded': 'Save_Person',
'aborted': 'Search_Person',
'preempted': 'Search_Person'
})
# Userdata input:
# person_location: PoseStamped (?)
# It is a SuperStateMachine (contains submachines) with these functionalities (draft):
# 1. Go to Person location
# 2. Ask Status
# 3. Register position
# 4. Save info
# What to do if fail?
smach.StateMachine.add('Save_Person',
Save_People_Emergency(),
transitions={
'succeeded':
'Get_Person_Desired_Object',
'aborted': 'Save_Person',
'preempted': 'Save_Person'
})
# The functionalities of this SuperSM are:
# 1. Ask the person what to fetch
# 2. Go and grab the object --> Similar with Pick-and-Place
# 2.1. Go to room
# 2.2. Find Object
# 2.3. Go to Object
# 2.4. Grab Object
# 2.5. Go to person
# 2.6. Give object --> Ungrab
#--> Database of objects and their location
# --> Manip/Grab
#
smach.StateMachine.add('Get_Person_Desired_Object',
Get_Person_Desired_Object(),
transitions={
'succeeded': 'Prepare_home',
'aborted': 'Get_Person_Desired_Object',
'preempted': 'Prepare_home'
})
#TODO: Define Entry room POI: userdata.nav_poi (?)
#Retrying to go to entry_door until is succeeded
smach.StateMachine.add('Prepare_home',
set_home_position(),
transitions={
'succeeded': 'SetHome',
'aborted': 'SetHome',
'preempted': 'SetHome'
})
smach.StateMachine.add('SetHome',
play_motion_sm(),
transitions={
'succeeded': 'Go_to_Entry_Door',
'aborted': 'SetHome',
'preempted': 'SetHome'
})
smach.StateMachine.add('Go_to_Entry_Door',
nav_to_poi('entry_door_init'),
transitions={
'succeeded':
'Wait_for_Ambulance_Person',
'aborted': 'Go_to_Entry_Door',
'preempted': 'Go_to_Entry_Door'
})
#What is Wait for Ambulance or People Mean? Person detection?
smach.StateMachine.add('Wait_for_Ambulance_Person',
DummyStateMachine(),
transitions={
'succeeded': 'Prepare_TTS_2',
'aborted': 'Go_to_Entry_Door',
'preempted': 'Go_to_Entry_Door'
})
smach.StateMachine.add(
'Prepare_TTS_2',
prepare_tts(
"Please Follow Me, I will guide you to the emergency"),
transitions={
'succeeded': 'Say_Ambulance',
'aborted': 'Say_Ambulance',
'preempted': 'Say_Ambulance'
})
smach.StateMachine.add('Say_Ambulance',
text_to_say(),
transitions={
'succeeded': 'Go_to_emergency_room_2',
'aborted': 'Go_to_emergency_room_2',
'preempted': 'Go_to_emergency_room_2'
})
#TODO: Define the name of the room to enter (defined by the OC)
#If Aborted (not supposed to), retry?
smach.StateMachine.add('Go_to_emergency_room_2',
nav_to_poi('emergency_room'),
transitions={
'succeeded': 'Wait_state_emergency',
'aborted': 'Go_to_emergency_room_2',
'preempted': 'Go_to_emergency_room_2'
})
smach.StateMachine.add('Wait_state_emergency',
DummyStateMachine(),
transitions={
'succeeded': 'Say_Finish',
'aborted': 'Say_Finish',
'preempted': 'Say_Finish'
})
smach.StateMachine.add(
'Say_Finish',
text_to_say('Here is the Emergency. I helped to save a life.'),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'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, poi_name=None):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['nav_to_poi_name'],
output_keys=['standard_error'])
with self:
self.userdata.tts_text = None
self.userdata.tts_wait_before_speaking = None
self.userdata.tts_lang = None
self.userdata.manip_motion_to_play = None
self.userdata.manip_time_to_play = None
self.userdata.skip_planning = False
smach.StateMachine.add('PrepareData',
prepareData(poi_name),
transitions={
'succeeded': 'check_can_pass',
'aborted': 'aborted'
})
# Check door state
smach.StateMachine.add('check_can_pass',
check_door_status(),
transitions={
'succeeded': 'home_position',
'aborted': 'say_open_door',
'door_too_far': 'say_too_far_from_door'
})
# Robot is too far from door
smach.StateMachine.add('say_too_far_from_door',
text_to_say("I'm too far from the door."),
transitions={
'succeeded': 'approach_to_door',
'aborted': 'approach_to_door'
})
# Approach to the door
smach.StateMachine.add('approach_to_door',
nav_to_coord("/base_link"),
transitions={
'succeeded': 'check_can_pass',
'aborted': 'check_can_pass'
})
# Robot ask to open the door
smach.StateMachine.add(
'say_open_door',
text_to_say("Can anyone open the door please?"),
transitions={
'succeeded': 'sleep_state',
'aborted': 'sleep_state'
})
# Sleep time before speak
smach.StateMachine.add('sleep_state',
Sleeper(5),
transitions={
'succeeded': 'check_can_pass',
'aborted': 'check_can_pass'
})
# Home position
smach.StateMachine.add('home_position',
play_motion_sm('home'),
transitions={
'succeeded': 'say_enter_room',
'aborted': 'say_enter_room',
'preempted': 'succeeded'
})
# We don't need to prepare the state, it takes the input_key directly
# Robot announces it is going through the door
smach.StateMachine.add('say_enter_room',
text_to_say("I am going to enter the room"),
transitions={
'succeeded': 'pass_door',
'aborted': 'pass_door'
})
#We pass the door
smach.StateMachine.add('pass_door',
pass_door(),
transitions={
'succeeded': 'enter_room',
'aborted': 'pass_door',
'preempted': 'preempted'
})
# Go to the poi in the other site of the door
smach.StateMachine.add('enter_room',
nav_to_poi(),
transitions={
'succeeded': 'succeeded',
'aborted': 'enter_room',
'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,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['did_pick', 'object_name', 'name_face'],
output_keys=[])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.did_pick = True
smach.StateMachine.add('look_to_point',
look_to_point(direction="up",
min_duration=1.0),
transitions={
'succeeded':
'prepare_say_search_person',
'preempted': 'preempted',
'aborted': 'aborted'
})
# Say search for person
smach.StateMachine.add('prepare_say_search_person',
prepare_searching(),
transitions={
'succeeded': 'say_search_person',
'aborted': 'search_for_person',
'preempted': 'preempted'
})
# Say search for person
smach.StateMachine.add('say_search_person',
text_to_say(),
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',
#go_find_person("party_room"),
recognize_face_concurrent(time_out=15),
transitions={
'succeeded': 'say_found_person',
'aborted': 'prepare_ask_for_person_back',
'preempted': 'preempted'
})
# We recognize the person
# Say found the person
smach.StateMachine.add('say_found_person',
text_to_say("I found you!", wait=False),
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': 'deliver_drink',
'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'
})
# Person recognition FAILS
# 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': 'wait_for_person_back',
'aborted': 'wait_for_person_back',
'preempted': 'preempted'
})
smach.StateMachine.add('wait_for_person_back',
detect_face(),
transitions={
'succeeded': 'deliver_drink',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Deliver part
# Deliver Drink
smach.StateMachine.add(
'deliver_drink',
text_to_say(
"Ok, I'm going to deliver the drink, please take it"),
transitions={
'succeeded': 'check_place_give',
'aborted': 'check_place_give',
'preempted': 'preempted'
})
# Check if we pick or ask the drink
smach.StateMachine.add('check_place_give',
check_place_give(),
transitions={
'succeeded': 'pregrasp_state',
'aborted': 'Give_Object',
'preempted': 'check_place_give'
})
# Place if we pick the drink - Pre-grasp position
smach.StateMachine.add('pregrasp_state',
play_motion_sm('pre_grasp',
skip_planning=True),
transitions={
'succeeded': 'succeeded',
'preempted': 'pregrasp_state',
'aborted': 'pregrasp_state'
})
# Give if we ask for drink
smach.StateMachine.add('Give_Object',
give_object(),
transitions={
'succeeded': 'succeeded',
'aborted': 'Give_Object',
'preempted': 'Give_Object'
})
def __init__(self):
sm = smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'])
with self:
smach.StateMachine.add(
'Say_Init',
text_to_say(
"Initializing and Preparing Emergency Test. Let's go!"),
transitions={'succeeded': 'Say_Enter_Successful'}) #Arms_home
# Put the arms at the 'Home' position, in order improve the security
# We prepare the information to go to the init door
self.userdata.manip_motion_to_play = 'home'
self.userdata.skip_planning = False
smach.StateMachine.add('Arms_Home',
play_motion_sm(),
transitions={
'succeeded': 'Say_Ready',
'aborted': 'Say_Ready',
'preempted': 'Say_Ready'
})
#Prepare the data for the speech: language and time to wait
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_lang = 'en_US'
smach.StateMachine.add(
'Say_Ready',
text_to_say(
'Emergency Situation Test is going to Start. Now I am going to enter the Room to help people in need.'
),
transitions={
'succeeded': 'Enter_Room_Arena',
'aborted': 'Enter_Room_Arena',
'preempted': 'Enter_Room_Arena'
})
# Pass the entry room
# TODO: Define the poi for the output of the room
# Pre: The robot should be in front of the Arena door (maybe we should change this, depending on the conditions)
smach.StateMachine.add('Enter_Room_Arena',
EnterRoomSM('entry_door_exit'),
transitions={
'succeeded': 'Say_Enter_Successful',
'aborted': 'Say_Ready',
'preempted': 'Search_Person'
})
smach.StateMachine.add(
'Say_Enter_Successful',
text_to_say(
'I have successfully entered the apartment. Now I am going to find and rescue the person in emergency.'
),
transitions={
'succeeded': 'Search_Person',
'aborted': 'Search_Person',
'preempted': 'Search_Person'
})
# The Location for the Emergency is not known --> Search for the emergency situation
# smach.StateMachine.add(
# 'Prepare_Poi_Emergency_1',
# prepare_poi_emergency('emergency_room'),
# transitions={'succeeded':'Go_to_emergency_room'})
# smach.StateMachine.add(
# 'Go_to_emergency_room',
# nav_to_poi(),
# transitions={'succeeded':'Say_Search_Person', 'aborted':'Go_to_emergency_room', 'preempted':'Go_to_emergency_room'})
# Userdata output keys:
# - person_location: PoseStamped/-->Pose<-- (?)
# Another state will be needed (maybe) to remap
# No need of face_recognition
# What if person not found? Re-search?
# smach.StateMachine.add(
# 'Say_Search_Person',
# text_to_say('Successfully entered the room. Now I am going to search for a person in an emergency situation.'),
# transitions={'succeeded':'Search_Person', 'aborted':'Search_Person', 'preempted':'Search_Person'})
smach.StateMachine.add('Search_Person',
Search_People_Emergency(),
transitions={
'succeeded': 'Save_Person',
'aborted': 'Search_Person',
'preempted': 'Search_Person'
})
# Userdata input:
# person_location: PoseStamped (?)
# It is a SuperStateMachine (contains submachines) with these functionalities (draft):
# 1. Go to Person location
# 2. Ask Status
# 3. Register position
# 4. Save info
# What to do if fail?
smach.StateMachine.add('Save_Person',
Save_People_Emergency(),
transitions={
'succeeded':
'Get_Person_Desired_Object',
'aborted': 'Save_Person',
'preempted': 'Save_Person'
})
# The functionalities of this SuperSM are:
# 1. Ask the person what to fetch
# 2. Go and grab the object --> Similar with Pick-and-Place
# 2.1. Go to room
# 2.2. Find Object
# 2.3. Go to Object
# 2.4. Grab Object
# 2.5. Go to person
# 2.6. Give object --> Ungrab
#--> Database of objects and their location
# --> Manip/Grab
#
smach.StateMachine.add('Get_Person_Desired_Object',
Get_Person_Desired_Object(),
transitions={
'succeeded': 'Ambulance_SM',
'aborted': 'Get_Person_Desired_Object',
'preempted': 'Ambulance_SM'
})
#Retrying to go to entry_door until is succeeded
smach.StateMachine.add('Ambulance_SM',
Ambulance_Detect_And_Go(),
transitions={
'succeeded': 'Final_State',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add(
'Final_State',
text_to_say(
'Emergency Situation Test 2014 Finished Successfully. Emergency Test'
),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'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.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):
sm = smach.StateMachine.__init__(self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['person_location_coord'])
with self:
self.userdata.name=""
smach.StateMachine.add(
'SetHome',
play_motion_sm('home'),
transitions={'succeeded':'Say_return_Person',
'aborted':'Say_return_Person',
'preempted':'Say_return_Person'})
smach.StateMachine.add(
'Say_return_Person',
text_to_say('I am going to the Entry Door for the Ambulance'),
transitions={'succeeded':'Go_to_Entry_Door', 'aborted':'Go_to_Entry_Door', 'aborted':'Go_to_Entry_Door'})
smach.StateMachine.add(
'Go_to_Entry_Door',
nav_to_poi('entry_door_init'),
transitions={'succeeded':'Say_Ambulance', 'aborted':'Say_Ambulance', 'preempted':'Say_Ambulance'})
#transitions={'succeeded':'Wait_for_Ambulance_Person', 'aborted':'Go_to_Entry_Door', 'preempted':'Go_to_Entry_Door'})
#What is Wait for Ambulance or People Mean? Person detection?
smach.StateMachine.add(
'Wait_for_Ambulance_Person',
Search_Ambulance_Face(),
#Search_Face_Determined('Where are you ambulance?'),
transitions={'succeeded':'Say_Ambulance', 'aborted':'Detect_Fail_Init', 'preempted':'Go_to_Entry_Door'})
smach.StateMachine.add(
'Detect_Fail_Init',
play_motion_sm('home'),
transitions={'succeeded':'Detect_Fail_Execute', 'aborted':'Detect_Fail_Execute', 'preempted':'Detect_Fail_Execute'})
smach.StateMachine.add(
'Detect_Fail_Execute',
detect_face(),
transitions={'succeeded': 'Say_Ambulance', 'aborted': 'Say_Ambulance'})
smach.StateMachine.add(
'Say_Ambulance',
text_to_say("Thank you for arriving as fast as possible. Please Follow Me, I will guide you to the emergency"),
transitions={'succeeded':'Prepare_Emergency_Final', 'aborted':'Prepare_Emergency_Final', 'preempted':'Prepare_Emergency_Final'})
#If Aborted (not supposed to), retry?
smach.StateMachine.add(
'Prepare_Emergency_Final',
prepare_poi_person_emergency(),
transitions={'succeeded':'Go_to_emergency_room_2', 'aborted':'Go_to_emergency_room_2', 'preempted':'Go_to_emergency_room_2'})
smach.StateMachine.add(
'Go_to_emergency_room_2',
nav_to_coord('/map'),
transitions={'succeeded':'Wait_state_emergency', 'aborted':'Go_to_emergency_room_2', 'preempted':'Go_to_emergency_room_2'})
smach.StateMachine.add(
'Wait_state_emergency',
DummyStateMachine(),
transitions={'succeeded':'Say_Finish', 'aborted':'Say_Finish', 'preempted':'Say_Finish'})
smach.StateMachine.add(
'Say_Finish',
text_to_say('Here is the person that is in an emergency situation, please proceed. I helped to save a life.'),
transitions={'succeeded':'succeeded', 'aborted':'aborted', 'preempted':'preempted'})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
output_keys=[],
input_keys=[])
with self:
#maybe i will have to learn again
smach.StateMachine.add('INIT_VAR',
waitstate(),
transitions={
'succeeded': 'play_motion_state',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('play_motion_state',
play_motion_sm('home', skip_planning=False),
transitions={
'succeeded': 'WAIT_0',
'preempted': 'WAIT_0',
'aborted': 'WAIT_0'
})
smach.StateMachine.add('WAIT_0',
waitstate(),
transitions={
'succeeded': 'say_0',
'aborted': 'say_0',
'preempted': 'preempted'
})
smach.StateMachine.add(
'say_0',
text_to_say(
"hello my name is reem, i'm really happy to be here and show you my movements. I come from Barcelona. I was created by pal robotics. I came here with my team reem at la salle.",
wait=False),
transitions={
'succeeded': 'play_motion_state_0',
'aborted': 'play_motion_state_0',
'preempted': 'play_motion_state_0'
})
smach.StateMachine.add('play_motion_state_0',
play_motion_sm('wave', skip_planning=False),
transitions={
'succeeded': 'WAIT_1',
'preempted': 'WAIT_1',
'aborted': 'WAIT_1'
})
smach.StateMachine.add('WAIT_1',
waitstate(),
transitions={
'succeeded': 'play_motion_state_1',
'aborted': 'play_motion_state_1',
'preempted': 'preempted'
})
smach.StateMachine.add('play_motion_state_1',
play_motion_sm('arms_t',
skip_planning=False),
transitions={
'succeeded': 'WAIT_2',
'preempted': 'WAIT_2',
'aborted': 'WAIT_2'
})
smach.StateMachine.add('WAIT_2',
waitstate(),
transitions={
'succeeded': 'play_motion_state_2',
'aborted': 'play_motion_state_2',
'preempted': 'preempted'
})
smach.StateMachine.add('play_motion_state_2',
play_motion_sm('wave', skip_planning=False),
transitions={
'succeeded': 'WAIT_3',
'preempted': 'WAIT_3',
'aborted': 'WAIT_3'
})
smach.StateMachine.add('WAIT_3',
waitstate(),
transitions={
'succeeded': 'play_motion_state_3',
'aborted': 'play_motion_state_3',
'preempted': 'preempted'
})
smach.StateMachine.add('play_motion_state_3',
play_motion_sm('brochure_tray',
skip_planning=False),
transitions={
'succeeded': 'WAIT_4',
'preempted': 'WAIT_4',
'aborted': 'WAIT_4'
})
smach.StateMachine.add('WAIT_4',
waitstate(),
transitions={
'succeeded': 'play_motion_state_4',
'aborted': 'play_motion_state_4',
'preempted': 'preempted'
})
smach.StateMachine.add('play_motion_state_4',
play_motion_sm('carry_basquet',
skip_planning=False),
transitions={
'succeeded': 'WAIT_5',
'preempted': 'WAIT_5',
'aborted': 'WAIT_5'
})
smach.StateMachine.add('WAIT_5',
waitstate(),
transitions={
'succeeded': 'play_motion_state_5',
'aborted': 'play_motion_state_5',
'preempted': 'preempted'
})
smach.StateMachine.add('play_motion_state_5',
play_motion_sm('shake_right',
skip_planning=False),
transitions={
'succeeded': 'WAIT_6',
'preempted': 'WAIT_6',
'aborted': 'WAIT_6'
})
smach.StateMachine.add('WAIT_6',
waitstate(),
transitions={
'succeeded': 'play_motion_state_6',
'aborted': 'play_motion_state_6',
'preempted': 'preempted'
})
smach.StateMachine.add('play_motion_state_6',
play_motion_sm('open_arms',
skip_planning=False),
transitions={
'succeeded': 'WAIT_0',
'preempted': 'WAIT_0',
'aborted': 'WAIT_0'
})
def __init__(self, poi_name = None):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['nav_to_poi_name'],
output_keys=['standard_error'])
with self:
self.userdata.tts_text = None
self.userdata.tts_wait_before_speaking = None
self.userdata.tts_lang = None
self.userdata.manip_motion_to_play = None
self.userdata.manip_time_to_play = None
self.userdata.skip_planning = False
smach.StateMachine.add('PrepareData',
prepareData(poi_name),
transitions={'succeeded':'check_can_pass', 'aborted':'aborted'})
# Check door state
smach.StateMachine.add('check_can_pass',
check_door_status(),
transitions={'succeeded': 'home_position',
'aborted': 'say_open_door',
'door_too_far': 'say_too_far_from_door'})
# Robot is too far from door
smach.StateMachine.add(
'say_too_far_from_door',
text_to_say("I'm too far from the door."),
transitions={'succeeded': 'approach_to_door', 'aborted': 'approach_to_door'})
# Approach to the door
smach.StateMachine.add(
'approach_to_door',
nav_to_coord("/base_link"),
transitions={'succeeded': 'check_can_pass', 'aborted': 'check_can_pass'})
# Robot ask to open the door
smach.StateMachine.add(
'say_open_door',
text_to_say("Can anyone open the door please?"),
transitions={'succeeded': 'sleep_state', 'aborted': 'sleep_state'})
# Sleep time before speak
smach.StateMachine.add(
'sleep_state',
Sleeper(5),
transitions={'succeeded': 'check_can_pass', 'aborted': 'check_can_pass'})
# Home position
smach.StateMachine.add(
'home_position',
play_motion_sm('home'),
transitions={'succeeded': 'say_enter_room', 'aborted': 'say_enter_room', 'preempted': 'succeeded'})
# We don't need to prepare the state, it takes the input_key directly
# Robot announces it is going through the door
smach.StateMachine.add(
'say_enter_room',
text_to_say("I am going to enter the room"),
transitions={'succeeded': 'pass_door', 'aborted': 'pass_door'})
#We pass the door
smach.StateMachine.add(
'pass_door',
pass_door(),
transitions={'succeeded': 'enter_room', 'aborted': 'pass_door', 'preempted': 'preempted'})
# Go to the poi in the other site of the door
smach.StateMachine.add(
'enter_room',
nav_to_poi(),
transitions={'succeeded': 'succeeded', 'aborted': 'enter_room', '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'})
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 = 1
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('play_motion_state',
play_motion_sm('home'),
transitions={
'succeeded': 'init_cocktail',
'preempted': 'init_cocktail',
'aborted': 'play_motion_state'
})
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("living_room"),
transitions={
'succeeded': 'Ask_order',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Ask Order -> Wave + Learn Person + Order
smach.StateMachine.add('Ask_order',
AskAllOrders(),
transitions={
'succeeded': 'execute_order',
'aborted': 'aborted'
})
# # Go to the storage_room
# smach.StateMachine.add(
# 'go_to_storage',
# nav_to_poi("storage_room"),
# transitions={'succeeded': 'execute_order', 'aborted': 'go_to_storage',
# 'preempted': 'preempted'})
# Execute the order
smach.StateMachine.add('execute_order',
ExecuteAllOrders(),
transitions={
'succeeded': 'say_leaving_arena',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Say leaving the arena
smach.StateMachine.add(
'say_leaving_arena',
text_to_say(
"I finished the cocktail party, I'm leaving the arena",
wait=False),
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.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")]
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': 'wait_inspection', 'aborted': 'home_position', 'preempted': 'succeeded'})
# Sleep for inspection
smach.StateMachine.add(
'wait_inspection',
Sleeper(5),
transitions={'succeeded': 'say_end_time_inspection', 'aborted': 'say_end_time_inspection', 'preempted': 'succeeded'})
# Indicate that we are ready to go out
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':'go_exit', 'aborted':'go_exit', 'preempted':'preempted'})
# Go to the exit
smach.StateMachine.add(
'go_exit',
GoToExit('exit_door'),
transitions= {'succeeded':'set_robot_position', 'aborted':'set_robot_position', 'preempted':'preempted'})
# Indicate that we are ready to go out
# smach.StateMachine.add(
# 'say_save_pos',
# text_to_say("Saving position"),
# 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'],
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,
outcomes=[
'succeeded', 'preempted', 'aborted', 'fail_grasp',
'fail_recognize'
],
input_keys=['object_name'],
output_keys=['object_position', 'object_detected_name'])
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_lang = ''
self.userdata.tts_wait_before_speak = ''
self.userdata.tts_text = ''
# Say start object recognition
smach.StateMachine.add(
'say_start_obj_recognition',
text_to_say(
"I'm going to start the Object recognition process.",
wait=False),
transitions={
'succeeded': 'object_recognition',
'aborted': 'object_recognition'
})
# Do object_recognition
smach.StateMachine.add('object_recognition',
recognize_object(),
transitions={
'succeeded':
'process_object_recognition',
'aborted': 'fail_recognize',
'preempted': 'preempted'
})
# Process the objects recognized
smach.StateMachine.add('process_object_recognition',
process_pick_location(),
transitions={
'succeeded': 'say_grasp_object',
'aborted': 'fail_recognize',
'preempted': 'preempted'
})
# Say grasp object
smach.StateMachine.add('say_grasp_object',
text_to_say("I'm going to grasp the object",
wait=False),
transitions={
'succeeded': 'grasp_object',
'aborted': 'aborted'
})
# Grasp the object
smach.StateMachine.add('grasp_object',
pick_object_sm(),
transitions={
'succeeded': 'succeeded',
'aborted': 'home_position',
'preempted': 'preempted'
})
# Home position
smach.StateMachine.add('home_position',
play_motion_sm('home', skip_planning=True),
transitions={
'succeeded': 'fail_grasp',
'aborted': 'home_position',
'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.nav_to_poi_name=''
self.userdata.manip_motion_to_play = ''
# Say Start basic Functionalities
smach.StateMachine.add(
'say_start_basic_functionalities',
text_to_say("I'm ready to start Basic Functionalities"),
transitions={'succeeded': 'say_going_pick_place', 'aborted': 'play_motion_state'}) # TODO before it was say_going_pick_place
# Say Go Pick and Place
smach.StateMachine.add(
'say_going_pick_place',
text_to_say("I'm going to the Pick and Place location"),
transitions={'succeeded': 'enter_start_door', 'aborted': 'enter_start_door'})
# Cross start door and go to intermediate point
smach.StateMachine.add(
'enter_start_door',
EnterRoomSM('kitchen_table'),
transitions={'succeeded': 'pick_timer', 'aborted': 'aborted',
'preempted': 'preempted'})
# Do pick and place + TimeOut
STATES = [PickPlaceSM()]
STATE_NAMES = ["do_pick_and_place"]
smach.StateMachine.add(
"pick_timer",
ConcurrenceTime(states=STATES, state_name=STATE_NAMES, timeout=180),
transitions={'succeeded': 'say_going_avoid',
'aborted':'say_going_avoid',
'time_ends': "timeout_pick_and_place"})
#
# smach.StateMachine.add(
# 'do_pick_and_place',
# PickPlaceSM(),
# transitions={'succeeded': 'play_motion_state_2', 'aborted': 'play_motion_state_2',
# 'preempted': 'preempted'})
# Say TimeOut
smach.StateMachine.add(
'timeout_pick_and_place',
text_to_say("The time for Pick and Place is finish"),
transitions={'succeeded': 'play_motion_state', 'aborted': 'play_motion_state',
'preempted': 'preempted'})
# Home position just in case that we are in pick position
smach.StateMachine.add(
'play_motion_state',
play_motion_sm('home', skip_planning=True),
transitions={'succeeded': 'say_going_avoid',
'preempted':'say_going_avoid',
'aborted':'play_motion_state'})
# Say Go Avoid that
smach.StateMachine.add(
'say_going_avoid',
text_to_say("I'm going to the Avoid that Area"),
transitions={'succeeded': 'go_avoid_that', 'aborted': 'go_avoid_that'})
# Go to avoid that
smach.StateMachine.add(
'go_avoid_that',
nav_to_poi('avoid_that'),
transitions={'succeeded': 'wait_avoid_that', 'aborted': 'go_avoid_that',
'preempted': 'preempted'})
# Wait for Go command
smach.StateMachine.add(
'wait_avoid_that',
ListenWordSM_Concurrent(SENTENCE_GO),
transitions={'succeeded': 'confirm_move_avoid_that', 'aborted': 'wait_avoid_that',
'preempted': 'preempted'})
smach.StateMachine.add(
'confirm_move_avoid_that',
acknowledgment("yes", "Okey, I move"),
transitions={'succeeded': 'avoid_timer', 'aborted': 'say_going_avoid',
'preempted': 'preempted'})
# Do avoid that + TimeOut
STATES = [Avoid_That()]
STATE_NAMES = ["do_avoid_that"]
smach.StateMachine.add(
"avoid_timer",
ConcurrenceTime(states=STATES, state_name=STATE_NAMES, timeout=180),
transitions={'succeeded': 'say_going_what_say',
'aborted':'say_going_what_say',
'time_ends': "timeout_avoid_that"})
# Say TimeOut
smach.StateMachine.add(
'timeout_avoid_that',
text_to_say("The time for Avoid That is finish"),
transitions={'succeeded': 'say_going_what_say', 'aborted': 'say_going_what_say',
'preempted': 'preempted'})
# Do avoid that
# smach.StateMachine.add(
# 'do_avoid_that',
# Avoid_That(),
# transitions={'succeeded': 'say_going_what_say', 'aborted': 'say_going_what_say',
# 'preempted': 'preempted'})
# Say Go What did you say
smach.StateMachine.add(
'say_going_what_say',
text_to_say("I'm ready to start the What did you say test"),
transitions={'succeeded': 'wait_what_did_you_say', 'aborted': 'wait_what_did_you_say'})
# Wait for Go command
smach.StateMachine.add(
'wait_what_did_you_say',
ListenWordSM_Concurrent(SENTENCE_GO),
transitions={'succeeded': 'confirm_move_what_say', 'aborted': 'wait_what_did_you_say',
'preempted': 'preempted'})
smach.StateMachine.add(
'confirm_move_what_say',
acknowledgment("yes", "Okey, I go"),
transitions={'succeeded': 'what_say_timer', 'aborted': 'what_say_timer',
'preempted': 'preempted'})
# Do what did you say + TimeOut
STATES = [WhatSaySM()]
STATE_NAMES = ["do_what_did_you_say"]
smach.StateMachine.add(
"what_say_timer",
ConcurrenceTime(states=STATES, state_name=STATE_NAMES, timeout=180),
transitions={'succeeded': 'go_to_door',
'aborted':'go_to_door',
'time_ends': "timeout_what_say"})
# Say TimeOut
smach.StateMachine.add(
'timeout_what_say',
text_to_say("The time for What did you say is finish"),
transitions={'succeeded': 'go_to_door', 'aborted': 'go_to_door',
'preempted': 'preempted'})
# Do what did you say
# smach.StateMachine.add(
# 'do_what_did_you_say',
# WhatSaySM(),
# transitions={'succeeded': 'say_finish_basic_functionalities', 'aborted': 'say_finish_basic_functionalities',
# 'preempted': 'preempted'})
# Go to the exit door
smach.StateMachine.add(
'go_to_door',
nav_to_poi('door_exit'),
transitions={'succeeded': 'go_to_exit', 'aborted': 'go_to_exit',
'preempted': 'preempted'})
# Go to the exit door
smach.StateMachine.add(
'go_to_exit',
nav_to_poi('door_B'),
transitions={'succeeded': 'say_finish_basic_functionalities', 'aborted': 'say_finish_basic_functionalities',
'preempted': 'preempted'})
# Say Finish basic Functionalities
smach.StateMachine.add(
'say_finish_basic_functionalities',
text_to_say("I finished Basic Functionalities"),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
def __init__(self):
sm = smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
smach.StateMachine.add(
'Say_Init',
text_to_say("Initializing and Preparing Emergency Test. Let's go!"),
transitions={'succeeded':'Arms_Home'})
# Put the arms at the 'Home' position, in order improve the security
# We prepare the information to go to the init door
self.userdata.manip_motion_to_play = 'home'
self.userdata.manip_time_to_play = 4.0
smach.StateMachine.add(
'Arms_Home',
play_motion_sm(),
transitions={'succeeded':'Prepare_TTS_1', 'aborted':'Prepare_TTS_1', 'preempted':'Prepare_TTS_1'})
#Prepare the data for the speech: language and time to wait
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_lang = 'en_US'
smach.StateMachine.add(
'Prepare_TTS_1',
prepare_tts("Now I am going to enter the Room to help people in need."),
transitions={'succeeded':'Say_Ready'})
smach.StateMachine.add(
'Say_Ready',
text_to_say(),
transitions={'succeeded':'Enter_Room_Arena', 'aborted':'Enter_Room_Arena', 'preempted':'Enter_Room_Arena'})
# Pass the entry room
# TODO: Define the poi for the output of the room
# Pre: The robot should be in front of the Arena door (maybe we should change this, depending on the conditions)
#smach.StateMachine.add(
# 'Prepare_Door_Out_Arena',
# prepare_poi_emergency('entry_door_exit'),
# transitions={'succeeded':'Enter_Room_Arena', 'aborted':'Enter_Room_Arena', 'preempted':'Enter_Room_Arena'})
smach.StateMachine.add(
'Enter_Room_Arena',
EnterRoomSM('entry_door_exit'),
transitions={'succeeded':'Say_Enter_Successful', 'aborted':'Prepare_TTS_1', 'preempted':'Prepare_Poi_Emergency_1'})
smach.StateMachine.add(
'Say_Enter_Successful',
text_to_say('Successfully entering the room. Now I am going to the Emergency Room.'),
transitions={'succeeded':'Prepare_Poi_Emergency_1', 'aborted':'Prepare_Poi_Emergency_1', 'preempted':'Prepare_Poi_Emergency_1'})
# Go to the emergency location
# TODO: Define the name of the room to enter (defined by the OC)
# If Aborted (not supposed to), retry?
smach.StateMachine.add(
'Prepare_Poi_Emergency_1',
prepare_poi_emergency('emergency_room'),
transitions={'succeeded':'Go_to_emergency_room'})
smach.StateMachine.add(
'Go_to_emergency_room',
nav_to_poi(),
transitions={'succeeded':'Search_Person', 'aborted':'Go_to_emergency_room', 'preempted':'Go_to_emergency_room'})
# Userdata output keys:
# - person_location: PoseStamped/-->Pose<-- (?)
# Another state will be needed (maybe) to remap
# No need of face_recognition
# What if person not found? Re-search?
smach.StateMachine.add(
'Search_Person',
Search_People_Emergency(),
transitions={'succeeded':'Save_Person', 'aborted':'Search_Person', 'preempted':'Search_Person'})
# Userdata input:
# person_location: PoseStamped (?)
# It is a SuperStateMachine (contains submachines) with these functionalities (draft):
# 1. Go to Person location
# 2. Ask Status
# 3. Register position
# 4. Save info
# What to do if fail?
smach.StateMachine.add(
'Save_Person',
Save_People_Emergency(),
transitions={'succeeded':'Get_Person_Desired_Object', 'aborted':'Save_Person', 'preempted':'Save_Person'})
# The functionalities of this SuperSM are:
# 1. Ask the person what to fetch
# 2. Go and grab the object --> Similar with Pick-and-Place
# 2.1. Go to room
# 2.2. Find Object
# 2.3. Go to Object
# 2.4. Grab Object
# 2.5. Go to person
# 2.6. Give object --> Ungrab
#--> Database of objects and their location
# --> Manip/Grab
#
smach.StateMachine.add(
'Get_Person_Desired_Object',
Get_Person_Desired_Object(),
transitions={'succeeded':'Prepare_home', 'aborted':'Get_Person_Desired_Object', 'preempted':'Prepare_home'})
#TODO: Define Entry room POI: userdata.nav_poi (?)
#Retrying to go to entry_door until is succeeded
smach.StateMachine.add(
'Prepare_home',
set_home_position(),
transitions={'succeeded':'SetHome', 'aborted':'SetHome', 'preempted':'SetHome'})
smach.StateMachine.add(
'SetHome',
play_motion_sm(),
transitions={'succeeded':'Go_to_Entry_Door', 'aborted':'SetHome', 'preempted':'SetHome'})
smach.StateMachine.add(
'Go_to_Entry_Door',
nav_to_poi('entry_door_init'),
transitions={'succeeded':'Wait_for_Ambulance_Person', 'aborted':'Go_to_Entry_Door', 'preempted':'Go_to_Entry_Door'})
#What is Wait for Ambulance or People Mean? Person detection?
smach.StateMachine.add(
'Wait_for_Ambulance_Person',
DummyStateMachine(),
transitions={'succeeded':'Prepare_TTS_2', 'aborted':'Go_to_Entry_Door', 'preempted':'Go_to_Entry_Door'})
smach.StateMachine.add(
'Prepare_TTS_2',
prepare_tts("Please Follow Me, I will guide you to the emergency"),
transitions={'succeeded':'Say_Ambulance', 'aborted':'Say_Ambulance', 'preempted':'Say_Ambulance'})
smach.StateMachine.add(
'Say_Ambulance',
text_to_say(),
transitions={'succeeded':'Go_to_emergency_room_2', 'aborted':'Go_to_emergency_room_2', 'preempted':'Go_to_emergency_room_2'})
#TODO: Define the name of the room to enter (defined by the OC)
#If Aborted (not supposed to), retry?
smach.StateMachine.add(
'Go_to_emergency_room_2',
nav_to_poi('emergency_room'),
transitions={'succeeded':'Wait_state_emergency', 'aborted':'Go_to_emergency_room_2', 'preempted':'Go_to_emergency_room_2'})
smach.StateMachine.add(
'Wait_state_emergency',
DummyStateMachine(),
transitions={'succeeded':'Say_Finish', 'aborted':'Say_Finish', 'preempted':'Say_Finish'})
smach.StateMachine.add(
'Say_Finish',
text_to_say('Here is the Emergency. I helped to save a life.'),
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'],
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'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
# Robot presentation: Little talk + wave gesture
STATES = [
text_to_say("Hi everybody! My name is REEM."),
play_motion_sm("wave")
]
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': 'object_recognition',
'aborted': "robot_presentation"
})
# Do object_recognition
smach.StateMachine.add('object_recognition',
dummy_recognize(),
transitions={
'succeeded': 'say_grasp_object',
'aborted': 'say_grasp_object',
'preempted': 'preempted'
})
# Say grasp object
smach.StateMachine.add(
'say_grasp_object',
text_to_say("I am hungry. I am going to pick the noodles"),
transitions={
'succeeded': 'grasp_object',
'aborted': 'grasp_object'
})
# Grasp the object
smach.StateMachine.add('grasp_object',
pick_object_sm(),
transitions={
'succeeded': 'say_why_object',
'aborted': 'play_motion_state',
'preempted': 'preempted'
})
# Say release object
smach.StateMachine.add(
'say_why_object',
text_to_say(
"I am a robot, I can't eat this. I am so sad! I am going to leave the noodles in the table",
wait=False),
transitions={
'succeeded': 'release_object',
'aborted': 'release_object'
})
# Release the object
smach.StateMachine.add('release_object',
place_object_sm(),
transitions={
'succeeded': 'robot_finish',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Say hard job + bow
STATES = [
text_to_say(
"Uff, this was a hard job. Thank you very much for your attention"
),
play_motion_sm("bow")
]
STATE_NAMES = ["say_hard_job", "motion_bow"]
outcome_map = {
'succeeded': {
"say_hard_job": 'succeeded',
"motion_bow": 'succeeded'
}
}
smach.StateMachine.add("robot_finish",
ConcurrenceRobocup(states=STATES,
state_names=STATE_NAMES,
outcome_map=outcome_map),
transitions={
'succeeded': 'say_rest',
'aborted': "say_rest"
})
# Say go rest
smach.StateMachine.add('say_rest',
text_to_say(
"I am going to rest for a few seconds",
wait=False),
transitions={
'succeeded': 'sleep_robot',
'aborted': 'sleep_robot'
})
# Sleep
smach.StateMachine.add('sleep_robot',
Sleeper(15),
transitions={
'succeeded': 'robot_presentation',
'preempted': 'robot_presentation',
'aborted': 'robot_presentation'
})
# Home position
smach.StateMachine.add('play_motion_state',
play_motion_sm('home'),
transitions={
'succeeded': 'say_grasp_object',
'preempted': 'say_grasp_object',
'aborted': 'say_grasp_object'
})
def __init__(self):
sm = smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
smach.StateMachine.add(
'Say_Init',
text_to_say("Initializing and Preparing Emergency Test. Let's go!"),
transitions={'succeeded':'Arms_Home'})
# Put the arms at the 'Home' position, in order improve the security
# We prepare the information to go to the init door
self.userdata.manip_motion_to_play = 'home'
self.userdata.skip_planning = False
smach.StateMachine.add(
'Arms_Home',
play_motion_sm(),
transitions={'succeeded':'Say_Ready', 'aborted':'Say_Ready', 'preempted':'Say_Ready'})
#Prepare the data for the speech: language and time to wait
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_lang = 'en_US'
smach.StateMachine.add(
'Say_Ready',
text_to_say('Emergency Situation Test is going to Start. Now I am going to enter the Room to help people in need.'),
transitions={'succeeded':'Enter_Room_Arena', 'aborted':'Enter_Room_Arena', 'preempted':'Enter_Room_Arena'})
# Pass the entry room
# TODO: Define the poi for the output of the room
# Pre: The robot should be in front of the Arena door (maybe we should change this, depending on the conditions)
smach.StateMachine.add(
'Enter_Room_Arena',
EnterRoomSM('entry_door_exit'),
transitions={'succeeded':'Say_Enter_Successful', 'aborted':'Say_Ready', 'preempted':'Search_Person'})
smach.StateMachine.add(
'Say_Enter_Successful',
text_to_say('I have successfully entered the apartment. Now I am going to find and rescue the person in emergency.'),
transitions={'succeeded':'Search_Person', 'aborted':'Search_Person', 'preempted':'Search_Person'})
# The Location for the Emergency is not known --> Search for the emergency situation
# smach.StateMachine.add(
# 'Prepare_Poi_Emergency_1',
# prepare_poi_emergency('emergency_room'),
# transitions={'succeeded':'Go_to_emergency_room'})
# smach.StateMachine.add(
# 'Go_to_emergency_room',
# nav_to_poi(),
# transitions={'succeeded':'Say_Search_Person', 'aborted':'Go_to_emergency_room', 'preempted':'Go_to_emergency_room'})
# Userdata output keys:
# - person_location: PoseStamped/-->Pose<-- (?)
# Another state will be needed (maybe) to remap
# No need of face_recognition
# What if person not found? Re-search?
# smach.StateMachine.add(
# 'Say_Search_Person',
# text_to_say('Successfully entered the room. Now I am going to search for a person in an emergency situation.'),
# transitions={'succeeded':'Search_Person', 'aborted':'Search_Person', 'preempted':'Search_Person'})
smach.StateMachine.add(
'Search_Person',
Search_People_Emergency(),
transitions={'succeeded':'Save_Person', 'aborted':'Search_Person', 'preempted':'Search_Person'})
# Userdata input:
# person_location: PoseStamped (?)
# It is a SuperStateMachine (contains submachines) with these functionalities (draft):
# 1. Go to Person location
# 2. Ask Status
# 3. Register position
# 4. Save info
# What to do if fail?
smach.StateMachine.add(
'Save_Person',
Save_People_Emergency(),
transitions={'succeeded':'Get_Person_Desired_Object', 'aborted':'Save_Person', 'preempted':'Save_Person'})
# The functionalities of this SuperSM are:
# 1. Ask the person what to fetch
# 2. Go and grab the object --> Similar with Pick-and-Place
# 2.1. Go to room
# 2.2. Find Object
# 2.3. Go to Object
# 2.4. Grab Object
# 2.5. Go to person
# 2.6. Give object --> Ungrab
#--> Database of objects and their location
# --> Manip/Grab
#
smach.StateMachine.add(
'Get_Person_Desired_Object',
Get_Person_Desired_Object(),
transitions={'succeeded':'Ambulance_SM', 'aborted':'Get_Person_Desired_Object', 'preempted':'Ambulance_SM'})
#Retrying to go to entry_door until is succeeded
smach.StateMachine.add(
'Ambulance_SM',
Ambulance_Detect_And_Go(),
transitions={'succeeded':'Final_State', 'aborted':'aborted', 'preempted':'preempted'})
smach.StateMachine.add(
'Final_State',
text_to_say('Emergency Situation Test 2014 Finished Successfully. Emergency Test'),
transitions={'succeeded':'succeeded', 'aborted':'aborted', 'preempted':'preempted'})
def __init__(self):
sm = smach.StateMachine.__init__(
self, outcomes=["succeeded", "preempted", "aborted"], input_keys=["person_location_coord"]
)
with self:
self.userdata.name = ""
smach.StateMachine.add(
"SetHome",
play_motion_sm("home"),
transitions={
"succeeded": "Say_return_Person",
"aborted": "Say_return_Person",
"preempted": "Say_return_Person",
},
)
smach.StateMachine.add(
"Say_return_Person",
text_to_say("I am going to the Entry Door for the Ambulance"),
transitions={
"succeeded": "Go_to_Entry_Door",
"aborted": "Go_to_Entry_Door",
"aborted": "Go_to_Entry_Door",
},
)
smach.StateMachine.add(
"Go_to_Entry_Door",
nav_to_poi("entry_door_init"),
transitions={
"succeeded": "Wait_for_Ambulance_Person",
"aborted": "Go_to_Entry_Door",
"preempted": "Go_to_Entry_Door",
},
)
# What is Wait for Ambulance or People Mean? Person detection?
smach.StateMachine.add(
"Wait_for_Ambulance_Person",
Search_Ambulance_Face(),
# Search_Face_Determined('Where are you ambulance?'),
transitions={
"succeeded": "Say_Ambulance",
"aborted": "Detect_Fail_Init",
"preempted": "Go_to_Entry_Door",
},
)
smach.StateMachine.add(
"Detect_Fail_Init",
play_motion_sm("home"),
transitions={
"succeeded": "Detect_Fail_Execute",
"aborted": "Detect_Fail_Execute",
"preempted": "Detect_Fail_Execute",
},
)
smach.StateMachine.add(
"Detect_Fail_Execute",
detect_face(),
transitions={"succeeded": "Say_Ambulance", "aborted": "Say_Ambulance"},
)
smach.StateMachine.add(
"Say_Ambulance",
text_to_say(
"Thank you for arriving as fast as possible. Please Follow Me, I will guide you to the emergency"
),
transitions={
"succeeded": "Prepare_Emergency_Final",
"aborted": "Prepare_Emergency_Final",
"preempted": "Prepare_Emergency_Final",
},
)
# If Aborted (not supposed to), retry?
smach.StateMachine.add(
"Prepare_Emergency_Final",
prepare_poi_person_emergency(),
transitions={
"succeeded": "Go_to_emergency_room_2",
"aborted": "Go_to_emergency_room_2",
"preempted": "Go_to_emergency_room_2",
},
)
smach.StateMachine.add(
"Go_to_emergency_room_2",
nav_to_coord("/map"),
transitions={
"succeeded": "Wait_state_emergency",
"aborted": "Go_to_emergency_room_2",
"preempted": "Go_to_emergency_room_2",
},
)
smach.StateMachine.add(
"Wait_state_emergency",
DummyStateMachine(),
transitions={"succeeded": "Say_Finish", "aborted": "Say_Finish", "preempted": "Say_Finish"},
)
smach.StateMachine.add(
"Say_Finish",
text_to_say(
"Here is the person that is in an emergency situation, please proceed. I helped to save a life."
),
transitions={"succeeded": "succeeded", "aborted": "aborted", "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.nav_to_poi_name=''
self.userdata.tts_lang = ''
self.userdata.tts_wait_before_speak = ''
self.userdata.tts_text = ''
self.userdata.loop_iterations = 1
self.userdata.did_unk = False
self.userdata.object_name = ['Pringles', 'Barritas']
# Say start Pick and Place
smach.StateMachine.add(
'say_start_pick_place',
text_to_say("I'm going to start Pick and Place test"),
transitions={'succeeded': 'say_go_location', 'aborted': 'say_go_location'})
# Say Going to location
smach.StateMachine.add(
'say_go_location',
text_to_say("I'm going to the location where I have to recognize some objects", wait=False),
transitions={'succeeded': 'prepare_location', 'aborted': 'prepare_location'})
# Prepare the poi for nav_to_poi
smach.StateMachine.add(
'prepare_location',
prepare_location(),
transitions={'succeeded': 'go_location', 'aborted': 'prepare_location',
'preempted': 'preempted'})
# Go to the location
smach.StateMachine.add(
'go_location',
nav_to_poi(),
transitions={'succeeded': 'recognize_object_and_pick', 'aborted': 'say_go_location',
'preempted': 'preempted'})
# recognize and pick object if found
smach.StateMachine.add(
'recognize_object_and_pick',
RecObjectAndPick(),
transitions={'succeeded': 'Process_Place_location',
'fail_grasp':'Process_Place_location',
'fail_recognize': 'try_again_recognition'})
# Prepare the place location
smach.StateMachine.add(
'Process_Place_location',
process_place_location(),
transitions={'succeeded':'say_go_second_location',
'aborted':'aborted'})
# Say start object recognition
# smach.StateMachine.add(
# 'say_start_obj_recognition',
# text_to_say("I'm going to start the Object recognition process.", wait=False),
# transitions={'succeeded': 'object_recognition', 'aborted': 'object_recognition'})
#
# # Do object_recognition
# smach.StateMachine.add(
# 'object_recognition',
# recognize_object(),
# transitions={'succeeded': 'process_object_recognition', 'aborted': 'try_again_recognition',
# 'preempted': 'preempted'})
#
# # Process the objects recognized
# smach.StateMachine.add(
# 'process_object_recognition',
# process_place_location(),
# transitions={'succeeded': 'say_grasp_object', 'aborted': 'say_grasp_object',
# 'preempted': 'preempted'})
# 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':'say_fail_recognize'})
# Say fail recognize objects
smach.StateMachine.add(
'say_fail_recognize',
text_to_say("I'm not able to recognized any object. I'm going to search for anything"),
transitions={'succeeded': 'prepare_unk_object', 'aborted': 'prepare_unk_object'})
# Prepare goal to pick any object
smach.StateMachine.add(
'prepare_unk_object',
prepare_unk_object(),
transitions={'succeeded': 'say_grasp_object', 'aborted': 'say_grasp_object'})
# Say grasp object
smach.StateMachine.add(
'say_grasp_object',
text_to_say("I'm going to grasp the object", wait=False),
transitions={'succeeded': 'grasp_object', 'aborted': 'grasp_object'})
# Grasp the object
smach.StateMachine.add(
'grasp_object',
pick_object_sm(),
transitions={'succeeded': 'say_go_second_location', 'aborted': 'home_position', #TODO: Change aborted to try again
'preempted': 'preempted'})
# Home position
smach.StateMachine.add(
'home_position',
play_motion_sm('home'),
transitions={'succeeded': 'say_grasp_object', 'aborted': 'home_position', #TODO: Change aborted to try again
'preempted': 'preempted'})
# Say go to second location
smach.StateMachine.add(
'say_go_second_location',
text_to_say("I'm going to the location where I should release the object", wait=False),
transitions={'succeeded': 'go_second_location', 'aborted': 'go_second_location'})
# Go the location - We need to go to the place to object category, so we assume that the
# object recognition will init the poi to the object must to go
smach.StateMachine.add(
'go_second_location',
nav_to_poi(),
transitions={'succeeded': 'say_release_obj', 'aborted': 'say_go_second_location',
'preempted': 'preempted'})
# Say release object
smach.StateMachine.add(
'say_release_obj',
text_to_say("I'm going to release the object", wait=True),
transitions={'succeeded': 'release_object', 'aborted': 'release_object'})
# Check if we pick the know or the unk object
smach.StateMachine.add(
'check_place_give',
check_object(),
transitions={'succeeded':'release_object', 'aborted':'pregrasp_state', 'preempted':'check_place_give'})
# Release the object
smach.StateMachine.add(
'release_object',
place_object_sm(),
transitions={'succeeded': 'play_motion_state', 'aborted': 'say_release_obj',
'preempted': 'preempted'})
# Pre-grasp position
smach.StateMachine.add(
'pregrasp_state',
play_motion_sm('pre_grasp', skip_planning=True),
transitions={'succeeded': 'play_motion_state', 'preempted':'play_motion_state',
'aborted':'pregrasp_state'})
# Home position
smach.StateMachine.add(
'play_motion_state',
play_motion_sm('home'),
transitions={'succeeded': 'say_end_pick_place', 'preempted':'say_end_pick_place',
'aborted':'say_end_pick_place'})
# Say end Pick and Place
smach.StateMachine.add(
'say_end_pick_place',
text_to_say("I finished the Pick and Place test"),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['did_pick', 'object_name', 'name_face'],
output_keys=[])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.did_pick = True
smach.StateMachine.add(
'look_to_point',
look_to_point(direction="up",min_duration=1.0),
transitions={'succeeded': 'prepare_say_search_person','preempted':'preempted', 'aborted':'aborted'})
# Say search for person
smach.StateMachine.add(
'prepare_say_search_person',
prepare_searching(),
transitions={'succeeded': 'say_search_person', 'aborted': 'search_for_person',
'preempted': 'preempted'})
# Say search for person
smach.StateMachine.add(
'say_search_person',
text_to_say(),
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',
#go_find_person("party_room"),
recognize_face_concurrent(time_out=15),
transitions={'succeeded': 'say_found_person', 'aborted': 'prepare_ask_for_person_back',
'preempted': 'preempted'})
# We recognize the person
# Say found the person
smach.StateMachine.add(
'say_found_person',
text_to_say("I found you!",wait=False),
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': 'deliver_drink', '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'})
# Person recognition FAILS
# 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': 'wait_for_person_back', 'aborted': 'wait_for_person_back', 'preempted': 'preempted'})
smach.StateMachine.add(
'wait_for_person_back',
detect_face(),
transitions={'succeeded': 'deliver_drink', 'aborted': 'aborted', 'preempted': 'preempted'})
# Deliver part
# Deliver Drink
smach.StateMachine.add(
'deliver_drink',
text_to_say("Ok, I'm going to deliver the drink, please take it"),
transitions={'succeeded': 'check_place_give', 'aborted': 'check_place_give',
'preempted': 'preempted'})
# Check if we pick or ask the drink
smach.StateMachine.add(
'check_place_give',
check_place_give(),
transitions={'succeeded':'pregrasp_state', 'aborted':'Give_Object', 'preempted':'check_place_give'})
# Place if we pick the drink - Pre-grasp position
smach.StateMachine.add(
'pregrasp_state',
play_motion_sm('pre_grasp', skip_planning=True),
transitions={'succeeded': 'succeeded', 'preempted':'pregrasp_state',
'aborted':'pregrasp_state'})
# Give if we ask for drink
smach.StateMachine.add(
'Give_Object',
give_object(),
transitions={'succeeded':'succeeded', 'aborted':'Give_Object', 'preempted':'Give_Object'})
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, ['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, poi_name=None):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['point_to_poi_name'],
output_keys=['standard_error'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
print "POINT TO POI IN-SELF"
self.userdata.standard_error = ''
self.userdata.point_to_coord_goal = [0.0, 0.0, 0.0]
smach.StateMachine.add('PrepareData',
prepareData(poi_name),
transitions={
'succeeded': 'translate_coord',
'aborted': 'aborted'
})
# We transform the poi to coordenades
smach.StateMachine.add('translate_coord',
translate_coord(),
transitions={
'succeeded': 'get_pose',
'aborted': 'aborted',
'preempted': 'preempted'
})
# We get current coodenades of robot in userdata 'current_robot_pose', 'current_robot_yaw', 'pose_current'
smach.StateMachine.add('get_pose',
GetPoseSubscribe(),
transitions={
'succeeded': 'get_yaw',
'aborted': 'aborted',
'preempted': 'preempted'
})
# We get current coodenades of robot in userdata 'current_robot_pose', 'current_robot_yaw', 'pose_current'
smach.StateMachine.add(
'get_yaw',
calculateYaw(), #output ['desired_angle']
transitions={
'succeeded': 'prepareNav',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Prepares to turn
smach.StateMachine.add('prepareNav',
prepare_nav_to_coord(),
transitions={
'succeeded': 'turn',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Turns
smach.StateMachine.add('turn',
nav_to_coord(),
transitions={
'succeeded': 'point_to_coord',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Point the coordenades
smach.StateMachine.add('point_to_coord',
play_motion_sm('point_forward'),
transitions={
'succeeded': 'Say_Pointing_Poi',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Say pointing
smach.StateMachine.add('Say_Pointing_Poi',
text_to_say('There, what I am pointing'),
transitions={
'succeeded': 'home_position',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Return home position
smach.StateMachine.add('home_position',
play_motion_sm('home'),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})