def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded','aborted', 'preempted'],
input_keys=[],
output_keys=[])
with self:
smach.StateMachine.add('Wait_search_1',
Wait_search(),
transitions={'succeeded':'Say_search', 'preempted':'preempted'})
smach.StateMachine.add('Say_search',
text_to_say('I am looking for the referee',wait=True),
transitions={'succeeded':'Move_head_Left', 'aborted':'aborted'})
smach.StateMachine.add('Move_head_Left',
look_to_point(direction="left", min_duration=3),
transitions={'succeeded':'Wait_search_2', 'aborted':'aborted'})
smach.StateMachine.add('Wait_search_2',
Wait_search(),
transitions={'succeeded':'Move_head_Right', 'preempted':'preempted'})
smach.StateMachine.add('Move_head_Right',
look_to_point(direction="front", min_duration=2),
transitions={'succeeded':'Wait_search_3', 'aborted':'aborted'})
smach.StateMachine.add('Wait_search_3',
Wait_search(),
transitions={'succeeded':'Move_head_Middle', 'preempted':'preempted'})
smach.StateMachine.add('Move_head_Middle',
look_to_point(direction="right", min_duration=2),
transitions={'succeeded':'Wait_search_1', 'aborted':'Wait_search_1'})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=[],
output_keys=[])
with self:
smach.StateMachine.add('Wait_search_1',
Wait_search(),
transitions={
'succeeded': 'Say_search',
'preempted': 'preempted'
})
smach.StateMachine.add('Say_search',
text_to_say('I am looking for the referee',
wait=True),
transitions={
'succeeded': 'Move_head_Left',
'aborted': 'aborted'
})
smach.StateMachine.add('Move_head_Left',
look_to_point(direction="left",
min_duration=3),
transitions={
'succeeded': 'Wait_search_2',
'aborted': 'aborted'
})
smach.StateMachine.add('Wait_search_2',
Wait_search(),
transitions={
'succeeded': 'Move_head_Right',
'preempted': 'preempted'
})
smach.StateMachine.add('Move_head_Right',
look_to_point(direction="front",
min_duration=2),
transitions={
'succeeded': 'Wait_search_3',
'aborted': 'aborted'
})
smach.StateMachine.add('Wait_search_3',
Wait_search(),
transitions={
'succeeded': 'Move_head_Middle',
'preempted': 'preempted'
})
smach.StateMachine.add('Move_head_Middle',
look_to_point(direction="right",
min_duration=2),
transitions={
'succeeded': 'Wait_search_1',
'aborted': 'Wait_search_1'
})
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,
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'
})