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:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.standard_error=''
self.userdata.nav_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 coordenates
smach.StateMachine.add(
'translate_coord',
translate_coord(),
transitions={'succeeded': 'move_to_coord', 'aborted': 'aborted', 'preempted': 'aborted'})
# Go to coordanates
smach.StateMachine.add(
'move_to_coord',
nav_to_coord(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})
def __init__(self, yawPlus = 0,direction = "left"):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
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!
self.userdata.standard_error=''
self.userdata.point_to_coord_goal=[0.0,0.0,0.0]
# We get current coodenades of robot in userdata 'current_robot_pose', 'current_robot_yaw', 'pose_current'
smach.StateMachine.add('get_pose',
GetPoseSubscribe(),
transitions={'succeeded': 'prepareNav', 'aborted': 'aborted', 'preempted': 'preempted'})
# Prepares to turn
smach.StateMachine.add('prepareNav',
prepare_nav_to_coord(yawPlus,direction),
transitions={'succeeded': 'turn', 'aborted': 'aborted', 'preempted': 'preempted'})
# Turns
smach.StateMachine.add('turn',
nav_to_coord(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
output_keys=['person_location', 'person_location_coord'])
with self:
self.userdata.emergency_location = []
# Some dummy TTS stuff
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
smach.StateMachine.add(
'Prepare_Say_Searching',
prepare_tts('Where are you? Give me signals, please.'),
transitions={'succeeded':'Say_Search', 'aborted':'Say_Search', 'preempted':'Say_Search'})
smach.StateMachine.add(
'Say_Search',
text_to_say(),
transitions={'succeeded':'Gesture_Recognition', 'aborted':'Gesture_Recognition', 'preempted':'Gesture_Recognition'})
# Search for a Wave Gesture
# Output_keys: gesture_detected: type Gesture
self.userdata.nav_to_coord = [0, 0, 0]
#Look Down
#Move head right/left
smach.StateMachine.add(
'Gesture_Recognition',
WaveDetection(),
transitions={'succeeded':'Say_Search','aborted':'Gesture_Recognition', 'preempted':'Gesture_Recognition'})
smach.StateMachine.add(
'Say_Search',
text_to_say('Oh! I have found you.'),
transitions={'succeeded':'Prepare_Go_To_Wave', 'aborted':'Prepare_Go_To_Wave', 'preempted':'Say_Search'})
smach.StateMachine.add(
'Prepare_Go_To_Wave',
prepare_go_to_wave(),
transitions={'succeeded':'Say_Go_to_Wave', 'aborted':'Say_Go_to_Wave', 'preempted':'Say_Go_to_Wave'})
smach.StateMachine.add(
'Say_Go_to_Wave',
text_to_say("I'm coming!"),
transitions={'succeeded':'Go_to_Wave', 'aborted':'Gesture_Recognition', 'preempted':'Gesture_Recognition'})
smach.StateMachine.add(
'Go_to_Wave',
nav_to_coord('/base_link'),
transitions={'succeeded':'Register_Position', 'aborted':'Go_to_Wave', 'preempted':'Go_to_Wave'})
smach.StateMachine.add(
'Register_Position',
get_current_robot_pose(),
transitions={'succeeded':'TreatPoseForCoord', 'aborted':'Register_Position', 'preempted':'Register_Position'},
remapping={'current_robot_pose':'person_location'})
smach.StateMachine.add(
'TreatPoseForCoord',
PoseToArray(),
transitions={'succeeded':'succeeded', 'aborted':'Register_Position', 'preempted':'Register_Position'})
def __init__(self, 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, yawPlus=0, direction="left"):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
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!
self.userdata.standard_error = ''
self.userdata.point_to_coord_goal = [0.0, 0.0, 0.0]
# We get current coodenades of robot in userdata 'current_robot_pose', 'current_robot_yaw', 'pose_current'
smach.StateMachine.add('get_pose',
GetPoseSubscribe(),
transitions={
'succeeded': 'prepareNav',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Prepares to turn
smach.StateMachine.add('prepareNav',
prepare_nav_to_coord(yawPlus, direction),
transitions={
'succeeded': 'turn',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Turns
smach.StateMachine.add('turn',
nav_to_coord(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
def __init__(self, x = None, y = None, yaw = None, distance_to_human = DISTANCE_TO_HUMAN):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['nav_to_coord_goal'],
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!
self.userdata.standard_error=''
smach.StateMachine.add('PrepareData',
prepareData(x, y, yaw, distance_to_human),
transitions={'succeeded':'move_to_coord',
'aborted':'aborted'})
smach.StateMachine.add(
'move_to_coord',
nav_to_coord(),
transitions={'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'})
def __init__(self, x = None, y = None, yaw = None, distance_to_human = DISTANCE_TO_HUMAN):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['nav_to_coord_goal'],
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!
self.userdata.standard_error=''
smach.StateMachine.add('PrepareData',
prepareData(x, y, yaw, distance_to_human),
transitions={'succeeded':'move_to_coord',
'aborted':'aborted'})
smach.StateMachine.add(
'move_to_coord',
nav_to_coord(),
transitions={'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'})
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:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.standard_error = ''
self.userdata.nav_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 coordenates
smach.StateMachine.add('translate_coord',
translate_coord(),
transitions={
'succeeded': 'move_to_coord',
'aborted': 'aborted',
'preempted': 'aborted'
})
# Go to coordanates
smach.StateMachine.add('move_to_coord',
nav_to_coord(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['distance'],
output_keys=[])
with self:
# Calculate the actual position
smach.StateMachine.add(
'get_actual_pos',
get_current_robot_pose(),
transitions={'succeeded': 'prepare_move_base', 'aborted': 'aborted', 'preempted': 'succeeded'})
# Prepare the new goal
smach.StateMachine.add(
'prepare_move_base',
prepare_move_base(),
transitions={'succeeded': 'go_to_point', 'aborted': 'aborted', 'preempted':'preempted'})
# Go to the new point
smach.StateMachine.add(
'go_to_point',
nav_to_coord(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})
def __init__(self):
smach.StateMachine.__init__(self, 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, 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,
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'])
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
# Find me Part
# 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 ther 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("find_me"),
transitions={
'succeeded': 'say_faces',
'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',
SearchFacesSM(),
transitions={
'succeeded': 'prepare_coord_person',
'aborted': 'ask_for_tc',
'preempted': 'preempted'
})
# Go to the person - We assume that find person will return the position for the person
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('/base_link'),
transitions={
'succeeded': 'say_found',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Say "I found you!" + Small Talk
smach.StateMachine.add('say_found',
text_to_say("I found you!"),
transitions={
'succeeded': 'ActivateASR',
'aborted': 'aborted'
})
# Ask for TC if we dont find him
smach.StateMachine.add(
'ask_for_tc',
text_to_say("I can't find you. Can you come to me?"),
transitions={
'succeeded': 'wait_for_tc',
'aborted': 'aborted'
})
# Wait for TC
smach.StateMachine.add('wait_for_tc',
detect_face(),
transitions={
'succeeded': 'say_found',
'aborted': 'aborted'
})
# Question Part ---------------------------------------------------------------------------------------------
# Init the asr service
# Activate the server
smach.StateMachine.add('ActivateASR',
ActivateASR(),
transitions={
'succeeded': 'check_loop',
'aborted': 'aborted',
'preempted': 'preempted'
})
# loop test - It checks the number of iterations
smach.StateMachine.add('check_loop',
checkLoop(),
transitions={
'succeeded': 'ask_next_question',
'aborted': 'aborted',
'preempted': 'preempted',
'end': 'DeactivateASR'
})
# Ask for next question
smach.StateMachine.add('ask_next_question',
text_to_say("I'm ready, ask me a question"),
transitions={
'succeeded': 'listen_question',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('listen_question',
ReadASR(),
transitions={
'succeeded': 'prepear_repeat',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('prepear_repeat',
prepear_repeat(),
transitions={
'succeeded': 'repeat_question',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Repeat question
smach.StateMachine.add('repeat_question',
text_to_say(),
transitions={
'succeeded': 'search_answer',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Search the answer
smach.StateMachine.add('search_answer',
SelectAnswer(),
transitions={
'succeeded': 'say_answer',
'aborted': 'say_answer',
'preempted': 'preempted',
'None': 'aborted'
})
# Say the answer
smach.StateMachine.add('say_answer',
text_to_say(),
transitions={
'succeeded': 'check_loop',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Deactivate ASR
smach.StateMachine.add('DeactivateASR',
DeactivateASR(),
transitions={
'succeeded': 'say_end',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add(
'say_end',
text_to_say("What did you say test finished"),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error'],
input_keys=['in_learn_person'])
with self:
self.userdata.tts_wait_before_speaking=0
self.userdata.tts_text=None
self.userdata.tts_lang=None
self.userdata.standar_error="ok"
self.userdata.word_to_listen=None
self.userdata.in_learn_person=person()
self.userdata.head_left_right=None
self.userdata.head_up_down=None
self.userdata.in_learn_person.targetId=1
print "HEY, THIS IS self.userdata.in_learn_person: " + str(self.userdata.in_learn_person)
def go_back_request(userdata,request):
start_request = NavigationGoBackRequest()
start_request.enable=True
start_request.meters=METERSBACK
return start_request
#maybe i will have to learn again
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={'succeeded': 'GO_BACK',
'aborted': 'aborted','preempted':'preempted'})
# i stop the service
def Cheack_Elevator_Stop(userdata, request):
start_request = EnableCheckElevatorRequest()
start_request.enable=False
return start_request
smach.StateMachine.add('STOP_CHECK_ELEVATOR',
ServiceState('/check_elevator/enable',
EnableCheckElevator,
request_cb = Cheack_Elevator_Stop),
transitions={'succeeded':'succeeded',
'preempted':'succeeded',
'aborted':'succeeded'})
# it will go from the lift
smach.StateMachine.add('GO_BACK',
ServiceState('/reverse',
NavigationGoBack,
request_cb = go_back_request),
transitions={'succeeded':'READ_TRACKER_TOPIC','aborted' : 'READ_TRACKER_TOPIC','preempted':'preempted'})
# i don't understant why is this goal here, i imagine for stop
smach.StateMachine.add('SEND_GOAL',
nav_to_coord_concurrent('/base_link'),
transitions={'succeeded':'READ_TRACKER_TOPIC', 'aborted':'WHERE_IS_IT','preempted':'preempted'})
smach.StateMachine.add('READ_TRACKER_TOPIC',
topic_reader(topic_name='/pipol_tracker_node/peopleSet',
topic_type=personArray,topic_time_out=60),
transitions={'succeeded':'WHERE_IS_IT',
'aborted':'READ_TRACKER_TOPIC',
'preempted':'preempted'},
remapping={'topic_output_msg': 'tracking_msg'})
# it will have to look if it can find the person, know it's only one try
smach.StateMachine.add('WHERE_IS_IT',
where_is_it(),
transitions={'i_dont_know':'SAY_COME_NEAR','ok_lets_go':'SAY_LETS_GO','preempted':'preempted'})
#this is when i don't remember the person
smach.StateMachine.add('SAY_COME_NEAR',
text_to_say(SAY_COME_NEAR),
transitions={'succeeded':'LEARN_AGAIN','aborted':'LEARN_AGAIN','preempted':'preempted'})
#hear i will learn the peron another time
smach.StateMachine.add('LEARN_AGAIN',
LearnPerson(learn_face=False),
transitions={'succeeded':'SAY_LETS_GO',
'aborted':'SAY_COME_NEAR','preempted':'preempted'})
smach.StateMachine.add('SAY_LETS_GO',
text_to_say(SAY_LETS_GO),
transitions={'succeeded':'TRACK_OPERATOR','aborted':'aborted','preempted':'preempted'})
smach.StateMachine.add('TRACK_OPERATOR',
FollowOperator(learn_if_lost=False,feedback=True),
transitions={'succeeded':'succeeded',
'lost':'LOST_CREATE_GOAL','preempted':'preempted'})
# i will prepere the navigation goal becouse i have lost the person
#the goal have to be a realy far goal
smach.StateMachine.add('LOST_CREATE_GOAL',create_nav_goal(),
transitions={'succeeded':'SEARCH_OPERATOR',
'aborted':'SEARCH_OPERATOR','preempted':'preempted'})
sm=smach.Concurrence(outcomes=['gesture_recongize', 'nav_finisheed','preempted'],
default_outcome='nav_finisheed',
input_keys=["in_learn_person",'nav_to_coord_goal'],
output_keys=['wave_position', 'wave_yaw_degree','standard_error'],
child_termination_cb = child_term_cb,
outcome_cb=out_cb_follow)
with sm:
sm.userdata.standar_error="ok"
#this goal will be a realy far goal
smach.Concurrence.add('SEND_GOAL',
nav_to_coord("/base_link"))
smach.Concurrence.add('SEARCH_OPERATOR_GESTURE',
Search_Wave_SM())
smach.StateMachine.add('SEARCH_OPERATOR', sm,
transitions={'gesture_recongize':'DEFAULT_POSITION',
'nav_finisheed':'SEARCH_OPERATOR_GESTURE_2','preempted':'preempted'})
smach.StateMachine.add('SEARCH_OPERATOR_GESTURE_2',
Search_Wave_SM(),
transitions={'succeeded':'DEFAULT_POSITION',
'aborted':'SEARCH_OPERATOR_GESTURE_2','preempted':'preempted', 'end_searching':'SEARCH_OPERATOR_GESTURE_2'})
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center","up"),
transitions={'succeeded': 'CREATE_GESTURE_NAV_GOAL','aborted':'CREATE_GESTURE_NAV_GOAL'})
smach.StateMachine.add('CREATE_GESTURE_NAV_GOAL',
create_nav_gesture_goal(),
transitions={'succeeded':'RECOGNIZE_PERSON',
'aborted':'aborted','preempted':'preempted'})
# smach.StateMachine.add('SAY_CROSSING',
# text_to_say(SAY_CROSSING),
# transitions={'succeeded':'GO_TO_GESTURE',
# 'aborted':'GO_TO_GESTURE','preempted':'preempted'})
# #if the navigation goal it's impossible it will be heare allways
# smach.StateMachine.add('GO_TO_GESTURE',
# nav_to_coord('/base_link'),
# transitions={'succeeded':'RECOGNIZE_PERSON',
# 'aborted':'RECOGNIZE_PERSON','preempted':'preempted'})
#
#when i'm whit the person i have to look if it's the person
smach.StateMachine.add('RECOGNIZE_PERSON',
LearnPerson(learn_face=False),
transitions={'succeeded':'SAY_GO_AGAIN',
'aborted':'aborted','preempted':'preempted'})
smach.StateMachine.add('SAY_GO_AGAIN',
text_to_say(SAY_GO_AGAIN),
transitions={'succeeded':'FOLLOW_AGAIN',
'aborted':'SEARCH_OPERATOR','preempted':'preempted'})
# hear i finish the state
smach.StateMachine.add('FOLLOW_AGAIN',
FollowOperator(feedback=True, learn_if_lost=True),
transitions={'succeeded':'succeeded',
'lost':'aborted','preempted':'preempted'})
def __init__(self):
smach.StateMachine.__init__(
self, ['succeeded', 'preempted', 'aborted'],
output_keys=['person_location', 'person_location_coord'])
with self:
self.userdata.emergency_location = []
# Some dummy TTS stuff
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
smach.StateMachine.add(
'Prepare_Say_Searching',
prepare_tts('Where are you? Give me signals, please.'),
transitions={
'succeeded': 'Say_Search',
'aborted': 'Say_Search',
'preempted': 'Say_Search'
})
smach.StateMachine.add('Say_Search',
text_to_say(),
transitions={
'succeeded': 'Gesture_Recognition',
'aborted': 'Gesture_Recognition',
'preempted': 'Gesture_Recognition'
})
# Search for a Wave Gesture
# Output_keys: gesture_detected: type Gesture
self.userdata.nav_to_coord = [0, 0, 0]
#Look Down
#Move head right/left
smach.StateMachine.add('Gesture_Recognition',
WaveDetection(),
transitions={
'succeeded': 'Say_Search',
'aborted': 'Gesture_Recognition',
'preempted': 'Gesture_Recognition'
})
smach.StateMachine.add('Say_Search',
text_to_say('Oh! I have found you.'),
transitions={
'succeeded': 'Prepare_Go_To_Wave',
'aborted': 'Prepare_Go_To_Wave',
'preempted': 'Say_Search'
})
smach.StateMachine.add('Prepare_Go_To_Wave',
prepare_go_to_wave(),
transitions={
'succeeded': 'Say_Go_to_Wave',
'aborted': 'Say_Go_to_Wave',
'preempted': 'Say_Go_to_Wave'
})
smach.StateMachine.add('Say_Go_to_Wave',
text_to_say("I'm coming!"),
transitions={
'succeeded': 'Go_to_Wave',
'aborted': 'Gesture_Recognition',
'preempted': 'Gesture_Recognition'
})
smach.StateMachine.add('Go_to_Wave',
nav_to_coord('/base_link'),
transitions={
'succeeded': 'Register_Position',
'aborted': 'Go_to_Wave',
'preempted': 'Go_to_Wave'
})
smach.StateMachine.add(
'Register_Position',
get_current_robot_pose(),
transitions={
'succeeded': 'TreatPoseForCoord',
'aborted': 'Register_Position',
'preempted': 'Register_Position'
},
remapping={'current_robot_pose': 'person_location'})
smach.StateMachine.add('TreatPoseForCoord',
PoseToArray(),
transitions={
'succeeded': 'succeeded',
'aborted': 'Register_Position',
'preempted': 'Register_Position'
})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.loop_iterations = 0
self.userdata.try_iterations = 1
self.userdata.gesture_name = ''
self.userdata.object_name = ""
self.userdata.manip_time_to_play = 4
self.userdata.did_pick = True
self.userdata.grammar_name = GRAMMAR_NAME
smach.StateMachine.add(
'init_cocktail',
text_to_say("Ready for cocktail party"),
transitions={'succeeded': 'learning_person', 'aborted': 'wait_for_door'})
# We wait for open door and go inside
smach.StateMachine.add(
'wait_for_door',
EnterRoomSM("party_room"),
transitions={'succeeded': 'say_search_wave', 'aborted': 'aborted', 'preempted': 'preempted'})
# Say Wave recognize
smach.StateMachine.add(
'say_search_wave',
text_to_say("I'm searching for people waving at me", wait=False),
transitions={'succeeded': 'wave_recognition', 'aborted': 'wave_recognition'})
# Gesture recognition -> Is anyone waving?
smach.StateMachine.add(
'wave_recognition',
WaveDetection(),
transitions={'succeeded': 'say_wave_recognize', 'aborted': 'ask_for_person',
'preempted': 'preempted'})
# Say Wave recognize
smach.StateMachine.add(
'say_wave_recognize',
text_to_say("Someone waved to me. I will go there", wait=False),
transitions={'succeeded': 'prepare_coord_wave', 'aborted': 'prepare_coord_wave'})
# Prepare the goal to the person that is waving
# TODO: it goes a little far to the person...
smach.StateMachine.add(
'prepare_coord_wave',
prepare_coord_wave(),
transitions={'succeeded': 'go_to_person_wave', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to the person -> we assume that gesture will return the position
smach.StateMachine.add(
'go_to_person_wave',
nav_to_coord('/base_link'),
transitions={'succeeded': 'learning_person', 'aborted': 'go_to_person_wave',
'preempted': 'preempted'})
# Ask for person if it can see anyone
smach.StateMachine.add(
'ask_for_person',
text_to_say("I can't see anyone. Can anyone come to me, please?"),
transitions={'succeeded': 'wait_for_person', 'aborted': 'ask_for_person',
'preempted': 'preempted'})
# Wait for person
smach.StateMachine.add(
'wait_for_person',
detect_face(),
transitions={'succeeded': 'learning_person', 'aborted': 'ask_for_person'})
# Learn Person -> Ask name + Face Recognition
# TODO: Set database
smach.StateMachine.add(
'learning_person',
SaveFaceSM(),
transitions={'succeeded': 'ask_order', 'aborted': 'learning_person',
'preempted': 'preempted'})
# Ask for order
smach.StateMachine.add(
'ask_order',
AskQuestionSM("What would you like to order?", GRAMMAR_NAME),
transitions={'succeeded': 'process_order', 'aborted': 'ask_order',
'preempted': 'preempted'})
# Process the answer
smach.StateMachine.add(
'process_order',
process_order(),
transitions={'succeeded': 'say_got_it', 'aborted': 'ask_order',
'preempted': 'preempted'})
# Say what he ask
smach.StateMachine.add(
'say_got_it',
text_to_say("I got it!"),
transitions={'succeeded': 'go_to_storage', 'aborted': 'go_to_storage',
'preempted': 'preempted'})
# Go to the storage_room
smach.StateMachine.add(
'go_to_storage',
nav_to_poi("storage_room"),
transitions={'succeeded': 'recognize_object_and_pick', 'aborted': 'go_to_storage',
'preempted': 'preempted'})
# Recognize and pick object if found
smach.StateMachine.add(
'recognize_object_and_pick',
RecObjectAndPick(),
transitions={'succeeded': 'go_to_party',
'fail_grasp':'go_to_party',
'fail_recognize': 'try_again_recognition'})
# Say start object recognition
# smach.StateMachine.add(
# 'say_start_obj_recognition',
# text_to_say("I'm going to start the Object recognition"),
# transitions={'succeeded': 'object_recognition', 'aborted': 'object_recognition'})
#
# # Do object_recognition
# smach.StateMachine.add(
# 'object_recognition',
# dummy_recognize(),
# transitions={'succeeded': 'say_grasp_object', 'aborted': 'say_release_obj',
# 'preempted': 'preempted'})
#
# # Say grasp object
# smach.StateMachine.add(
# 'say_grasp_order',
# text_to_say("I'm going to grasp it"),
# transitions={'succeeded': 'grasp_food_order', 'aborted': 'Grasp_fail_Ask_Person',
# 'preempted': 'preempted'})
#
# # Grasp Object
# smach.StateMachine.add(
# 'grasp_food_order',
# pick_object_sm(),
# transitions={'succeeded': 'go_to_party', 'aborted': 'Grasp_fail_Ask_Person',
# 'preempted': 'preempted'})
# We don't recognized the object
smach.StateMachine.add(
'try_again_recognition',
checkRecognition(),
transitions={'succeeded': 'recognize_object_and_pick', 'aborted': 'recognize_object_and_pick',
'preempted': 'preempted', 'end':'say_fail_recognize'})
# Say fail recognize objects
smach.StateMachine.add(
'say_fail_recognize',
text_to_say("I'm not able to recognized any object."),
transitions={'succeeded': 'Grasp_fail_Ask_Person', 'aborted': 'Grasp_fail_Ask_Person'})
# Ask for grasp object
smach.StateMachine.add(
'Grasp_fail_Ask_Person',
ask_give_object_grasping(),
remapping={'object_to_grasp':'object_name'},
transitions={'succeeded':'Rest_arm', 'aborted':'Rest_arm', 'preempted':'Rest_arm'})
smach.StateMachine.add(
'Rest_arm',
play_motion_sm('rest_object_right'),
transitions={'succeeded':'change_did_pick', 'aborted':'change_did_pick', 'preempted':'change_did_pick'})
smach.StateMachine.add(
'change_did_pick',
change_did_pick(),
transitions={'succeeded':'go_to_party', 'aborted':'go_to_party', 'preempted':'go_to_party'})
# Go to the party room
smach.StateMachine.add(
'go_to_party',
nav_to_poi('party_room'),
transitions={'succeeded': 'say_search_person', 'aborted': 'go_to_party',
'preempted': 'preempted'})
# Say search for person
smach.StateMachine.add(
'say_search_person',
text_to_say("I'm going to search the person who ordered me"),
transitions={'succeeded': 'search_for_person', 'aborted': 'search_for_person',
'preempted': 'preempted'})
# Search for person -> He could change his position
smach.StateMachine.add(
'search_for_person',
recognize_face_concurrent(),
transitions={'succeeded': 'say_found_person', 'aborted': 'prepare_ask_for_person_back',
'preempted': 'preempted'})
# Say found the person
smach.StateMachine.add(
'say_found_person',
text_to_say("I found you!"),
transitions={'succeeded': 'prepare_coord_order', 'aborted': 'aborted',
'preempted': 'preempted'})
# Prepare the goal to the person that ask for the order
smach.StateMachine.add(
'prepare_coord_order',
prepare_coord_order(),
transitions={'succeeded': 'go_to_person_order', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to person
smach.StateMachine.add(
'go_to_person_order',
nav_to_coord('/base_link'),
transitions={'succeeded': 'deliver_drink', 'aborted': 'aborted',
'preempted': 'preempted'})
# Ask for person if it can see anyone
smach.StateMachine.add(
'prepare_ask_for_person_back',
prepare_ask_person_back(),
transitions={'succeeded': 'ask_for_person_back', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'ask_for_person_back',
text_to_say(),
transitions={'succeeded': 'deliver_drink', 'aborted': 'aborted', 'preempted': 'preempted'})
# Deliver Drink
smach.StateMachine.add(
'deliver_drink',
text_to_say("I'm going to deliver the drink"),
transitions={'succeeded': 'Give_Object', 'aborted': 'Give_Object',
'preempted': 'preempted'})
# Check if we pick or ask the drink
smach.StateMachine.add(
'check_place_give',
check_place_give(),
transitions={'succeeded':'Place_Object', 'aborted':'Give_Object', 'preempted':'check_place_give'})
# Place if we pick the drink
smach.StateMachine.add(
'Place_Object',
place_object_sm(),
transitions={'succeeded':'check_loop', 'aborted':'Place_Object', 'preempted':'Place_Object'})
# Give if we ask for drink
smach.StateMachine.add(
'Give_Object',
give_object(),
transitions={'succeeded':'check_loop', 'aborted':'Give_Object', 'preempted':'Give_Object'})
# End of loop?
smach.StateMachine.add(
'check_loop',
checkLoop(),
transitions={'succeeded': 'wave_recognition', 'aborted': 'aborted',
'preempted': 'preempted', 'end':'leaving_arena'})
# Say leaving the arena
smach.StateMachine.add(
'say_leaving_arena',
text_to_say("I finished the cocktail party, I'm leaving the arena"),
transitions={'succeeded': 'Give_Object', 'aborted': 'Give_Object',
'preempted': 'preempted'})
# Leaving the arena
smach.StateMachine.add(
'leaving_arena',
nav_to_poi('leave_arena'),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})
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'],
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'],
input_keys=['person_location', 'person_location_coord'])
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
smach.StateMachine.add(
'Ask_Question',
text_to_say(text='What would you like me to bring?'),
transitions={
'succeeded': 'Listen_Question',
'aborted': 'Ask_Question',
'preempted': 'Listen_Question'
})
# Ask for the object to bring to the person
# Output:
# - string 'userSaid'
# - actiontag[] 'asr_userSaid_tags'
# TODO: grammar for the Emergency Situation -- Get_Person_Desired_Object
smach.StateMachine.add('Listen_Question',
ListenToSM(grammar='robocup/emergency'),
transitions={
'succeeded': 'Process_Tags',
'aborted': 'Ask_Question',
'preempted': 'Ask_Question'
})
# Get the output from AskQuestionSM, process it, and search in the yaml file for the location of the object asked
# Input keys: actiontag[] 'asr_userSaid_tags'
# Output keys: object
smach.StateMachine.add('Process_Tags',
Process_Tags(),
transitions={
'succeeded': 'Say_go_Kitchen',
'aborted': 'Ask_Question',
'aborted': 'Ask_Question'
})
smach.StateMachine.add(
'Say_go_Kitchen',
text_to_say(
'I am Going to the Kitchen for an object, Stay Here until I give you the object'
),
transitions={
'succeeded': 'Go_To_Object_Place',
'aborted': 'Go_To_Object_Place',
'aborted': 'Go_To_Object_Place'
})
smach.StateMachine.add('Go_To_Object_Place',
nav_to_poi('kitchen'),
transitions={
'succeeded': 'Grasp_fail_Ask_Person',
'aborted': 'Grasp_fail_Ask_Person',
'preempted': 'Grasp_fail_Ask_Person'
})
self.userdata.time_grasp = 0.0
smach.StateMachine.add('Grasping_with_timeout',
grasping_with_timeout(),
transitions={
'succeeded': 'Prepare_Go_To_Person',
'time_out': 'Grasp_fail_Ask_Person'
})
# sm_conc = smach.Concurrence(outcomes=['succeeded', 'time_out'],
# default_outcome='succeeded',
# input_keys=['object_to_grasp, time_grasp'],
# child_termination_cb = child_term_cb,
# outcome_cb = out_cb)
#
# with sm_conc:
# sm_conc.add(
# 'Find_and_grab_object',
# #Find_and_grab_object(),
# DummyStateMachine())
# sm_conc.add(
# 'Time_State',
# Time_State())
#
# smach.StateMachine.add('GRASP_CONCURRENCE',
# sm_conc,
# transitions={'succeeded':'Prepare_Go_To_Person',
# 'time_out':'Grasp_fail_Ask_Person'})
#Find Object + Grab Object SM
# smach.StateMachine.add(
# 'Find_and_grab_object',
# #Find_and_grab_object(),
# DummyStateMachine(),
# transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'Grasp_fail_Ask_Person'})
smach.StateMachine.add('Grasp_fail_Ask_Person',
ask_give_object_grasping(),
transitions={
'succeeded': 'Rest_arm',
'aborted': 'Rest_arm',
'preempted': 'Rest_arm'
})
smach.StateMachine.add('Rest_arm',
play_motion_sm('rest_object_right'),
transitions={
'succeeded': 'Prepare_Go_To_Person',
'aborted': 'Prepare_Go_To_Person',
'preempted': 'Prepare_Go_To_Person'
})
#Go to person
smach.StateMachine.add('Prepare_Go_To_Person',
prepare_poi_person_emergency(),
transitions={
'succeeded': 'Go_To_Person',
'aborted': 'Go_To_Person',
'preempted': 'Go_To_Person'
})
#TODO: POI For Person in Emergency -- From SearchPeople SM -
smach.StateMachine.add(
'Go_To_Person',
#DummyStateMachine(),
#nav_to_poi(),
nav_to_coord('/map'),
transitions={
'succeeded': 'Say_Give_Object',
'aborted': 'Say_Give_Object',
'preempted': 'Say_Give_Object'
})
smach.StateMachine.add(
'Say_Give_Object',
text_to_say('I am going to give you the Object you want.'),
transitions={
'succeeded': 'Give_object_arm',
'aborted': 'Give_object_arm',
'preempted': 'Give_object_arm'
})
smach.StateMachine.add('Give_object_arm',
play_motion_sm('give_object_right'),
transitions={
'succeeded': 'Give_Object',
'aborted': 'Give_Object',
'preempted': 'Give_Object'
})
#Give the grabbed object to the person
smach.StateMachine.add(
'Give_Object',
#DummyStateMachine(),
move_hands_form(hand_pose_name='pre_grasp', hand_side='right'),
transitions={
'succeeded': 'Give_Object_2',
'aborted': 'Give_Object',
'preempted': 'Give_Object'
})
smach.StateMachine.add(
'Give_Object_2',
#DummyStateMachine(),
move_hands_form(hand_pose_name='full_open', hand_side='right'),
transitions={
'succeeded': 'Say_Rescue_stay',
'aborted': 'Give_Object_2',
'preempted': 'Give_Object_2'
})
smach.StateMachine.add(
'Say_Rescue_stay',
text_to_say(
'Please Stay here I am going to call for the Ambulance'),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'aborted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(
self, ['succeeded', 'preempted', 'aborted'],
input_keys=['person_location', 'person_location_coord'])
pose_place = PoseStamped()
pose_place.header.frame_id = '/base_link'
pose_place.pose.position.x = 0.0
pose_place.pose.position.y = 0.0
pose_place.pose.position.z = 1.0
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
self.userdata.object_failed = False
self.userdata.object_name = None
smach.StateMachine.add(
'Ask_Question',
text_to_say(text='What would you like me to bring?'),
transitions={
'succeeded': 'Listen_Question',
'aborted': 'Ask_Question',
'preempted': 'Listen_Question'
})
smach.StateMachine.add('Listen_Question',
ListenToSM(grammar='robocup/emergency'),
transitions={
'succeeded': 'Process_Tags',
'aborted': 'Ask_Question',
'preempted': 'Ask_Question'
})
# Get the output from AskQuestionSM, process it, and search in the yaml file for the location of the object asked
# Input keys: actiontag[] 'asr_userSaid_tags'
# Output keys: object
smach.StateMachine.add('Process_Tags',
Process_Tags(),
transitions={
'succeeded': 'Search_Object',
'aborted': 'Ask_Question',
'aborted': 'Ask_Question'
})
smach.StateMachine.add(
'Search_Object',
object_detection_and_grasping_sm(),
transitions={
'succeeded': 'Say_return_Person',
'fail_object_detection': 'Grasp_failed_prepare',
'fail_object_grasping': 'Grasp_failed_prepare',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'object_name': 'object_to_grasp'})
smach.StateMachine.add(
'Grasp_failed_prepare',
Fail_Detection_Grasping(),
transitions={'succeeded': 'Grasp_fail_Ask_Person'})
# TODO: Saying to where to robot is heading.
# smach.StateMachine.add(
# 'Say_go_Place',
# text_to_say('I am Going to the Kitchen for an object, Stay Here until I give you the object'),
# transitions={'succeeded':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place'})
# smach.StateMachine.add(
# 'Go_To_Object_Place',
# nav_to_poi(),
# transitions={'succeeded':'Say_got_to_Kitchen', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'Grasp_fail_Ask_Person'})
# smach.StateMachine.add(
# 'Say_got_to_Kitchen',
# text_to_say('I am in the Kitchen, I am going to grasp fail ask person'),
# transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person'})
#
# self.userdata.time_grasp = 0.0
# smach.StateMachine.add('Grasping_with_timeout',
# grasping_with_timeout(),
# transitions={'succeeded':'Prepare_Go_To_Person', 'time_out':'Grasp_fail_Ask_Person'})
smach.StateMachine.add('Grasp_fail_Ask_Person',
ask_give_object_grasping(),
transitions={
'succeeded': 'Rest_arm',
'aborted': 'Rest_arm',
'preempted': 'Rest_arm'
})
smach.StateMachine.add('Rest_arm',
play_motion_sm('rest_object_right'),
transitions={
'succeeded': 'Say_return_Person',
'aborted': 'Say_return_Person',
'preempted': 'Say_return_Person'
})
smach.StateMachine.add(
'Say_return_Person',
text_to_say('I am preparing to go back to the person'),
transitions={
'succeeded': 'Prepare_Go_To_Person',
'aborted': 'Prepare_Go_To_Person',
'aborted': 'Prepare_Go_To_Person'
})
#Go to person
smach.StateMachine.add('Prepare_Go_To_Person',
prepare_poi_person_emergency(),
transitions={
'succeeded': 'Go_To_Person',
'aborted': 'Go_To_Person',
'preempted': 'Go_To_Person'
})
#TODO: POI For Person in Emergency -- From SearchPeople SM -
smach.StateMachine.add('Go_To_Person',
nav_to_coord('/map'),
transitions={
'succeeded': 'Say_Give_Object',
'aborted': 'Say_Give_Object',
'preempted': 'Say_Give_Object'
})
smach.StateMachine.add(
'Say_Give_Object',
text_to_say('I am going to give you the Object you asked.'),
transitions={
'succeeded': 'Select_next_state_grasping',
'aborted': 'Select_next_state_grasping',
'preempted': 'Select_next_state_grasping'
})
smach.StateMachine.add('Select_next_state_grasping',
Select_Grasp_Next_State(),
transitions={
'state_failed': 'Give_object_arm',
'state_succeeded': 'Give_object_both'
})
smach.StateMachine.add('Give_object_both',
place_object_sm(pose_place),
transitions={
'succeeded': 'Say_Rescue_stay',
'aborted': 'Say_Rescue_stay',
'preempted': 'preempted'
})
smach.StateMachine.add('Give_object_arm',
play_motion_sm('give_object_right'),
transitions={
'succeeded': 'Give_Object',
'aborted': 'Give_Object',
'preempted': 'Give_Object'
})
#Give the grabbed object to the person
smach.StateMachine.add('Give_Object',
move_hands_form(hand_pose_name='pre_grasp',
hand_side='right'),
transitions={
'succeeded': 'Give_Object_2',
'aborted': 'Give_Object',
'preempted': 'Give_Object'
})
smach.StateMachine.add('Give_Object_2',
move_hands_form(hand_pose_name='full_open',
hand_side='right'),
transitions={
'succeeded': 'Say_Rescue_stay',
'aborted': 'Give_Object_2',
'preempted': 'Give_Object_2'
})
smach.StateMachine.add(
'Say_Rescue_stay',
text_to_say(
'Please Stay here I am going to call for the Ambulance'),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'aborted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.loop_iterations = 0
self.userdata.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
# Find me Part
# 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 ther 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("find_me"),
transitions={'succeeded': 'say_faces', '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',
SearchFacesSM(),
transitions={'succeeded': 'prepare_coord_person', 'aborted': 'ask_for_tc',
'preempted': 'preempted'})
# Go to the person - We assume that find person will return the position for the person
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('/base_link'),
transitions={'succeeded': 'say_found', 'aborted': 'aborted',
'preempted': 'preempted'})
# Say "I found you!" + Small Talk
smach.StateMachine.add(
'say_found',
text_to_say("I found you!"),
transitions={'succeeded': 'ActivateASR', 'aborted': 'aborted'})
# Ask for TC if we dont find him
smach.StateMachine.add(
'ask_for_tc',
text_to_say("I can't find you. Can you come to me?"),
transitions={'succeeded': 'wait_for_tc', 'aborted': 'aborted'})
# Wait for TC
smach.StateMachine.add(
'wait_for_tc',
detect_face(),
transitions={'succeeded': 'say_found', 'aborted': 'aborted'})
# Question Part ---------------------------------------------------------------------------------------------
# Init the asr service
# Activate the server
smach.StateMachine.add('ActivateASR',
ActivateASR(),
transitions={'succeeded': 'check_loop', 'aborted': 'aborted', 'preempted': 'preempted'})
# loop test - It checks the number of iterations
smach.StateMachine.add(
'check_loop',
checkLoop(),
transitions={'succeeded': 'ask_next_question', 'aborted': 'aborted',
'preempted': 'preempted', 'end':'DeactivateASR'})
# Ask for next question
smach.StateMachine.add(
'ask_next_question',
text_to_say("I'm ready, ask me a question"),
transitions={'succeeded': 'listen_question', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'listen_question',
ReadASR(),
transitions={'succeeded': 'prepear_repeat', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'prepear_repeat',
prepear_repeat(),
transitions={'succeeded': 'repeat_question', 'aborted': 'aborted',
'preempted': 'preempted'})
# Repeat question
smach.StateMachine.add(
'repeat_question',
text_to_say(),
transitions={'succeeded': 'search_answer', 'aborted': 'aborted',
'preempted': 'preempted'})
# Search the answer
smach.StateMachine.add(
'search_answer',
SelectAnswer(),
transitions={'succeeded': 'say_answer', 'aborted': 'say_answer',
'preempted': 'preempted', 'None': 'aborted'})
# Say the answer
smach.StateMachine.add(
'say_answer',
text_to_say(),
transitions={'succeeded': 'check_loop', 'aborted': 'aborted',
'preempted': 'preempted'})
# Deactivate ASR
smach.StateMachine.add('DeactivateASR',
DeactivateASR(),
transitions={'succeeded': 'say_end', 'aborted': 'aborted', 'preempted': 'preempted'})
smach.StateMachine.add(
'say_end',
text_to_say("What did you say test finished"),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
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, 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'])
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'],
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, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['delete_database'],
output_keys=['name_face', 'object_name'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
# Say Wave recognize
smach.StateMachine.add(
'say_search_wave',
text_to_say("I'm searching for people waving at me", wait=False),
transitions={'succeeded': 'wave_recognition', 'aborted': 'wave_recognition'})
# Gesture recognition -> Is anyone waving?
smach.StateMachine.add(
'wave_recognition',
WaveDetection(time_for_wave=20),
transitions={'succeeded': 'say_wave_recognize', 'aborted': 'ask_for_person',
'preempted': 'preempted'})
# Say Wave recognize
smach.StateMachine.add(
'say_wave_recognize',
text_to_say("Someone waved to me. I will go there", wait=False),
transitions={'succeeded': 'prepare_coord_wave', 'aborted': 'prepare_coord_wave'})
# Person Recognize
# 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': 'say_approach_person', 'aborted': 'say_not_approach_person',
'preempted': 'preempted'})
#FAIL Navigation
# Say can't approach person
smach.StateMachine.add(
'say_not_approach_person',
text_to_say("I can't reach where you are. Can anyone come to me, please?"),
transitions={'succeeded': 'wait_for_person', 'aborted': 'wait_for_person'})
# Say approach person
smach.StateMachine.add(
'say_approach_person',
text_to_say("I found you. I am looking at you. I will enroll your face."),
transitions={'succeeded': 'wait_for_person', 'aborted': 'wait_for_person'})
# FAIL Person Recognize
# 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': 'wait_for_person',
'preempted': 'preempted'})
# Wait for person
smach.StateMachine.add(
'wait_for_person',
detect_face(),
transitions={'succeeded': 'say_learn_person', 'aborted': 'ask_for_person'})
# Say approach person
smach.StateMachine.add(
'say_learn_person',
text_to_say("I have detected your face."),
transitions={'succeeded': 'learning_person', 'aborted': 'learning_person'})
# Learn Person -> Ask name + Face Recognition
smach.StateMachine.add(
'learning_person',
SaveFaceSM(time_enroll=5),
transitions={'succeeded': 'ask_order', 'aborted': 'learning_person',
'preempted': 'preempted'})
# Ask for order
smach.StateMachine.add(
'ask_order',
AskQuestionSM("What would you like to order?", GRAMMAR_NAME, True),
transitions={'succeeded': 'process_order', 'aborted': 'ask_order',
'preempted': 'preempted'})
# Process the answer
smach.StateMachine.add(
'process_order',
process_order(),
transitions={'succeeded': 'say_got_it', 'aborted': 'ask_order',
'preempted': 'preempted'})
smach.StateMachine.add(
'calibrate',
CalibrateASR(),
transitions={'succeeded': 'ask_order', 'aborted': 'ask_order',
'preempted': 'preempted'})
# Say what he ask
smach.StateMachine.add(
'say_got_it',
text_to_say(),
transitions={'succeeded': 'succeeded', 'aborted': 'succeeded',
'preempted': 'preempted'})
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, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['delete_database'],
output_keys=['name_face', 'object_name'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
# Say Wave recognize
smach.StateMachine.add(
'say_search_wave',
text_to_say("I'm searching for people waving at me", wait=False),
transitions={'succeeded': 'wave_recognition', 'aborted': 'wave_recognition'})
# Gesture recognition -> Is anyone waving?
smach.StateMachine.add(
'wave_recognition',
WaveDetection(time_for_wave=20),
transitions={'succeeded': 'say_wave_recognize', 'aborted': 'ask_for_person',
'preempted': 'preempted'})
# Say Wave recognize
smach.StateMachine.add(
'say_wave_recognize',
text_to_say("Someone waved to me. I will go there", wait=False),
transitions={'succeeded': 'prepare_coord_wave', 'aborted': 'prepare_coord_wave'})
# Person Recognize
# 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'})
# FAIL Person Recognize
# Ask for person if it can see anyone
smach.StateMachine.add(
'ask_for_person',
text_to_say("I can't see anyone. Can anyone come to me, please?"),
transitions={'succeeded': 'wait_for_person', 'aborted': 'ask_for_person',
'preempted': 'preempted'})
# Wait for person
smach.StateMachine.add(
'wait_for_person',
detect_face(),
transitions={'succeeded': 'learning_person', 'aborted': 'ask_for_person'})
# Learn Person -> Ask name + Face Recognition
smach.StateMachine.add(
'learning_person',
SaveFaceSM(time_enroll=5),
transitions={'succeeded': 'ask_order', 'aborted': 'learning_person',
'preempted': 'preempted'})
# Ask for order
smach.StateMachine.add(
'ask_order',
AskQuestionSM("What would you like to order?", GRAMMAR_NAME),
transitions={'succeeded': 'process_order', 'aborted': 'ask_order',
'preempted': 'preempted'})
# Process the answer
smach.StateMachine.add(
'process_order',
process_order(),
transitions={'succeeded': 'say_got_it', 'aborted': 'ask_order',
'preempted': 'preempted'})
# Say what he ask
smach.StateMachine.add(
'say_got_it',
text_to_say("I got it!"),
transitions={'succeeded': '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=''
# Params for drop faces - always will be the same
self.userdata.name_database = "find_me_database"
self.userdata.purgeAll = True
# Init the state machine - Drop faces in the database
smach.StateMachine.add(
'init_database',
drop_faces(),
remapping={'name':'name_database'},
transitions={'succeeded': 'save_face', 'aborted': 'aborted',
'preempted': 'preempted'})
# Save the data from the TC
# We need one state machine that listen the name and then learn the face
smach.StateMachine.add(
'save_face',
SaveFaceSM(),
transitions={'succeeded': 'prepare_location', 'aborted': 'aborted',
'preempted': 'preempted'})
# Prepare the info to go to the room
smach.StateMachine.add(
'prepare_location',
prepare_location(),
transitions={'succeeded': 'go_location', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to the location
smach.StateMachine.add(
'go_location',
nav_to_poi(),
transitions={'succeeded': 'search_faces', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go around the room, it the robot find the TC return success
smach.StateMachine.add(
'search_faces',
SearchFacesSM(),
transitions={'succeeded': 'prepare_coord_person', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to the person - We assume that find person will return the position for the person
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(),
transitions={'succeeded': 'prepare_say_found', 'aborted': 'aborted',
'preempted': 'preempted'})
# Say "I found you!"
smach.StateMachine.add(
'prepare_say_found',
prepare_say_found(),
transitions={'succeeded': 'say_found', 'aborted': 'aborted', 'preempted': 'preempted'})
smach.StateMachine.add(
'say_found',
text_to_say(),
transitions={'succeeded': 'gesture_recognition', 'aborted': 'aborted'})
# Recognize the direction
smach.StateMachine.add(
'gesture_recognition',
DummyStateMachine(),
transitions={'succeeded': 'go_side', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to the direction
smach.StateMachine.add(
'go_side',
DummyStateMachine(),
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':'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,
outcomes=["succeeded", "preempted", "aborted"],
input_keys=["delete_database"],
output_keys=["name_face", "object_name"],
)
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
# Say Wave recognize
smach.StateMachine.add(
"say_search_wave",
text_to_say("I'm searching for people waving at me", wait=False),
transitions={"succeeded": "wave_recognition", "aborted": "wave_recognition"},
)
# Gesture recognition -> Is anyone waving?
smach.StateMachine.add(
"wave_recognition",
WaveDetection(time_for_wave=20),
transitions={"succeeded": "say_wave_recognize", "aborted": "ask_for_person", "preempted": "preempted"},
)
# Say Wave recognize
smach.StateMachine.add(
"say_wave_recognize",
text_to_say("Someone waved to me. I will go there", wait=False),
transitions={"succeeded": "prepare_coord_wave", "aborted": "prepare_coord_wave"},
)
# Person Recognize
# 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": "say_approach_person",
"aborted": "say_not_approach_person",
"preempted": "preempted",
},
)
# FAIL Navigation
# Say can't approach person
smach.StateMachine.add(
"say_not_approach_person",
text_to_say("I can't reach where you are. Can anyone come to me, please?"),
transitions={"succeeded": "wait_for_person", "aborted": "wait_for_person"},
)
# Say approach person
smach.StateMachine.add(
"say_approach_person",
text_to_say("I found you. I am looking at you. I will enroll your face."),
transitions={"succeeded": "wait_for_person", "aborted": "wait_for_person"},
)
# FAIL Person Recognize
# 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": "wait_for_person", "preempted": "preempted"},
)
# Wait for person
smach.StateMachine.add(
"wait_for_person",
detect_face(),
transitions={"succeeded": "say_learn_person", "aborted": "ask_for_person"},
)
# Say approach person
smach.StateMachine.add(
"say_learn_person",
text_to_say("I have detected your face."),
transitions={"succeeded": "learning_person", "aborted": "learning_person"},
)
# Learn Person -> Ask name + Face Recognition
smach.StateMachine.add(
"learning_person",
SaveFaceSM(time_enroll=5),
transitions={"succeeded": "ask_order", "aborted": "learning_person", "preempted": "preempted"},
)
# Ask for order
smach.StateMachine.add(
"ask_order",
AskQuestionSM("What would you like to order?", GRAMMAR_NAME, True),
transitions={"succeeded": "process_order", "aborted": "ask_order", "preempted": "preempted"},
)
# Process the answer
smach.StateMachine.add(
"process_order",
process_order(),
transitions={"succeeded": "say_got_it", "aborted": "ask_order", "preempted": "preempted"},
)
smach.StateMachine.add(
"calibrate",
CalibrateASR(),
transitions={"succeeded": "ask_order", "aborted": "ask_order", "preempted": "preempted"},
)
# Say what he ask
smach.StateMachine.add(
"say_got_it",
text_to_say(),
transitions={"succeeded": "succeeded", "aborted": "succeeded", "preempted": "preempted"},
)
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error'],
input_keys=['in_learn_person'])
with self:
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_text = None
self.userdata.tts_lang = None
self.userdata.standar_error = "ok"
self.userdata.word_to_listen = None
self.userdata.in_learn_person = 1
def go_back_request(userdata, request):
start_request = NavigationGoBackRequest()
start_request.enable = True
start_request.meters = METERSBACK
return start_request
#maybe i will have to learn again
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={
'succeeded': 'GO_BACK',
'aborted': 'aborted',
'preempted': 'preempted'
})
def Cheack_Elevator_Stop(userdata, request):
start_request = EnableCheckElevatorRequest()
start_request.enable = False
return start_request
smach.StateMachine.add('STOP_CHECK_ELEVATOR',
ServiceState(
'/check_elevator/enable',
EnableCheckElevator,
request_cb=Cheack_Elevator_Stop),
transitions={
'succeeded': 'succeeded',
'preempted': 'succeeded',
'aborted': 'succeeded'
})
# it will go from the lift
smach.StateMachine.add('GO_BACK',
ServiceState('/reverse',
NavigationGoBack,
request_cb=go_back_request),
transitions={
'succeeded': 'SEND_GOAL',
'aborted': 'SEND_GOAL',
'preempted': 'preempted'
})
smach.StateMachine.add('SEND_GOAL',
nav_to_coord_concurrent('/base_link'),
transitions={
'succeeded': 'WHERE_IS_IT',
'aborted': 'WHERE_IS_IT',
'preempted': 'preempted'
})
# it will go out from the lift
smach.StateMachine.add('WHERE_IS_IT',
where_is_it(),
transitions={
'i_dont_know': 'SAY_COME_NEAR',
'ok_lets_go': 'SAY_LETS_GO',
'preempted': 'preempted'
})
#this is when i don't remember the person
smach.StateMachine.add('SAY_COME_NEAR',
text_to_say(SAY_COME_NEAR),
transitions={
'succeeded': 'LEARN_AGAIN',
'aborted': 'LEARN_AGAIN',
'preempted': 'preempted'
})
#hear i will learn the peron another time
smach.StateMachine.add('LEARN_AGAIN',
learn_again(),
transitions={
'succeeded': 'SAY_LETS_GO',
'aborted': 'SAY_COME_NEAR',
'preempted': 'preempted'
})
smach.StateMachine.add('SAY_LETS_GO',
text_to_say(SAY_LETS_GO),
transitions={
'succeeded': 'TRACK_OPERATOR',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('TRACK_OPERATOR',
FollowOperator(),
transitions={
'succeeded': 'succeeded',
'lost': 'LOST_CREATE_GOAL',
'preempted': 'preempted'
})
# i will prepere the navigation goal becouse i have lost the person
#the goal have to be a realy far goal
smach.StateMachine.add('LOST_CREATE_GOAL',
create_nav_goal(),
transitions={
'succeeded': 'SEARCH_OPERATOR',
'aborted': 'SEARCH_OPERATOR',
'preempted': 'preempted'
})
sm = smach.Concurrence(
outcomes=['gesture_recongize', 'nav_finisheed', 'preempted'],
default_outcome='nav_finisheed',
input_keys=["in_learn_person"],
child_termination_cb=child_term_cb,
outcome_cb=out_cb_follow,
output_keys=[])
with sm:
sm.userdata.standar_error = "ok"
#this goal will be a realy far goal
smach.Concurrence.add('SEND_GOAL', nav_to_coord())
smach.Concurrence.add('SEARCH_OPERATOR_GESTURE',
gesture_recogniton())
smach.StateMachine.add('SEARCH_OPERATOR',
sm,
transitions={
'gesture_recongize':
'CREATE_GESTURE_NAV_GOAL',
'nav_finisheed': 'SEARCH_OPERATOR',
'preempted': 'preempted'
})
smach.StateMachine.add('CREATE_GESTURE_NAV_GOAL',
create_nav_gesture_goal(),
transitions={
'succeeded': 'GO_TO_GESTURE',
'aborted': 'aborted',
'preempted': 'preempted'
})
#if the navigation goal it's impossible it will be heare allways
smach.StateMachine.add('GO_TO_GESTURE',
nav_to_coord('/base_link'),
transitions={
'succeeded': 'RECOGNIZE_PERSON',
'aborted': 'RECOGNIZE_PERSON',
'preempted': 'preempted'
})
#when i'm whit the person i have to look if it's the person
smach.StateMachine.add('RECOGNIZE_PERSON',
recognize_person(),
transitions={
'succeeded': 'SAY_GO_AGAIN',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('SAY_GO_AGAIN',
text_to_say(SAY_GO_AGAIN),
transitions={
'succeeded': 'FOLLOW_AGAIN',
'aborted': 'SEARCH_OPERATOR',
'preempted': 'preempted'
})
# hear i finish the state
smach.StateMachine.add('FOLLOW_AGAIN',
FollowOperator(),
transitions={
'succeeded': 'succeeded',
'lost': 'aborted',
'preempted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error'],
input_keys=['in_learn_person'])
with self:
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_text = None
self.userdata.tts_lang = None
self.userdata.standar_error = "ok"
self.userdata.word_to_listen = None
self.userdata.in_learn_person = person()
self.userdata.head_left_right = None
self.userdata.head_up_down = None
self.userdata.in_learn_person.targetId = 1
print "HEY, THIS IS self.userdata.in_learn_person: " + str(
self.userdata.in_learn_person)
def go_back_request(userdata, request):
start_request = NavigationGoBackRequest()
start_request.enable = True
start_request.meters = METERSBACK
return start_request
#maybe i will have to learn again
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={
'succeeded': 'GO_BACK',
'aborted': 'aborted',
'preempted': 'preempted'
})
# i stop the service
def Cheack_Elevator_Stop(userdata, request):
start_request = EnableCheckElevatorRequest()
start_request.enable = False
return start_request
smach.StateMachine.add('STOP_CHECK_ELEVATOR',
ServiceState(
'/check_elevator/enable',
EnableCheckElevator,
request_cb=Cheack_Elevator_Stop),
transitions={
'succeeded': 'succeeded',
'preempted': 'succeeded',
'aborted': 'succeeded'
})
# it will go from the lift
smach.StateMachine.add('GO_BACK',
ServiceState('/reverse',
NavigationGoBack,
request_cb=go_back_request),
transitions={
'succeeded': 'READ_TRACKER_TOPIC',
'aborted': 'READ_TRACKER_TOPIC',
'preempted': 'preempted'
})
# i don't understant why is this goal here, i imagine for stop
smach.StateMachine.add('SEND_GOAL',
nav_to_coord_concurrent('/base_link'),
transitions={
'succeeded': 'READ_TRACKER_TOPIC',
'aborted': 'WHERE_IS_IT',
'preempted': 'preempted'
})
smach.StateMachine.add(
'READ_TRACKER_TOPIC',
topic_reader(topic_name='/pipol_tracker_node/peopleSet',
topic_type=personArray,
topic_time_out=60),
transitions={
'succeeded': 'WHERE_IS_IT',
'aborted': 'READ_TRACKER_TOPIC',
'preempted': 'preempted'
},
remapping={'topic_output_msg': 'tracking_msg'})
# it will have to look if it can find the person, know it's only one try
smach.StateMachine.add('WHERE_IS_IT',
where_is_it(),
transitions={
'i_dont_know': 'SAY_COME_NEAR',
'ok_lets_go': 'SAY_LETS_GO',
'preempted': 'preempted'
})
#this is when i don't remember the person
smach.StateMachine.add('SAY_COME_NEAR',
text_to_say(SAY_COME_NEAR),
transitions={
'succeeded': 'LEARN_AGAIN',
'aborted': 'LEARN_AGAIN',
'preempted': 'preempted'
})
#hear i will learn the peron another time
smach.StateMachine.add('LEARN_AGAIN',
LearnPerson(learn_face=False),
transitions={
'succeeded': 'SAY_LETS_GO',
'aborted': 'SAY_COME_NEAR',
'preempted': 'preempted'
})
smach.StateMachine.add('SAY_LETS_GO',
text_to_say(SAY_LETS_GO),
transitions={
'succeeded': 'TRACK_OPERATOR',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('TRACK_OPERATOR',
FollowOperator(learn_if_lost=False,
feedback=True),
transitions={
'succeeded': 'succeeded',
'lost': 'LOST_CREATE_GOAL',
'preempted': 'preempted'
})
# i will prepere the navigation goal becouse i have lost the person
#the goal have to be a realy far goal
smach.StateMachine.add('LOST_CREATE_GOAL',
create_nav_goal(),
transitions={
'succeeded': 'SEARCH_OPERATOR',
'aborted': 'SEARCH_OPERATOR',
'preempted': 'preempted'
})
sm = smach.Concurrence(
outcomes=['gesture_recongize', 'nav_finisheed', 'preempted'],
default_outcome='nav_finisheed',
input_keys=["in_learn_person", 'nav_to_coord_goal'],
output_keys=[
'wave_position', 'wave_yaw_degree', 'standard_error'
],
child_termination_cb=child_term_cb,
outcome_cb=out_cb_follow)
with sm:
sm.userdata.standar_error = "ok"
#this goal will be a realy far goal
smach.Concurrence.add('SEND_GOAL', nav_to_coord("/base_link"))
smach.Concurrence.add('SEARCH_OPERATOR_GESTURE',
Search_Wave_SM())
smach.StateMachine.add('SEARCH_OPERATOR',
sm,
transitions={
'gesture_recongize': 'DEFAULT_POSITION',
'nav_finisheed':
'SEARCH_OPERATOR_GESTURE_2',
'preempted': 'preempted'
})
smach.StateMachine.add('SEARCH_OPERATOR_GESTURE_2',
Search_Wave_SM(),
transitions={
'succeeded': 'DEFAULT_POSITION',
'aborted': 'SEARCH_OPERATOR_GESTURE_2',
'preempted': 'preempted',
'end_searching':
'SEARCH_OPERATOR_GESTURE_2'
})
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center", "up"),
transitions={
'succeeded': 'CREATE_GESTURE_NAV_GOAL',
'aborted': 'CREATE_GESTURE_NAV_GOAL'
})
smach.StateMachine.add('CREATE_GESTURE_NAV_GOAL',
create_nav_gesture_goal(),
transitions={
'succeeded': 'RECOGNIZE_PERSON',
'aborted': 'aborted',
'preempted': 'preempted'
})
# smach.StateMachine.add('SAY_CROSSING',
# text_to_say(SAY_CROSSING),
# transitions={'succeeded':'GO_TO_GESTURE',
# 'aborted':'GO_TO_GESTURE','preempted':'preempted'})
# #if the navigation goal it's impossible it will be heare allways
# smach.StateMachine.add('GO_TO_GESTURE',
# nav_to_coord('/base_link'),
# transitions={'succeeded':'RECOGNIZE_PERSON',
# 'aborted':'RECOGNIZE_PERSON','preempted':'preempted'})
#
#when i'm whit the person i have to look if it's the person
smach.StateMachine.add('RECOGNIZE_PERSON',
LearnPerson(learn_face=False),
transitions={
'succeeded': 'SAY_GO_AGAIN',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('SAY_GO_AGAIN',
text_to_say(SAY_GO_AGAIN),
transitions={
'succeeded': 'FOLLOW_AGAIN',
'aborted': 'SEARCH_OPERATOR',
'preempted': 'preempted'
})
# hear i finish the state
smach.StateMachine.add('FOLLOW_AGAIN',
FollowOperator(feedback=True,
learn_if_lost=True),
transitions={
'succeeded': 'succeeded',
'lost': 'aborted',
'preempted': 'preempted'
})
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'
})
