def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['object_array'])
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=None
self.userdata.standard_error='OK'
self.userdata.object_name = ''
self.userdata.object_index = 0
# Process order
smach.StateMachine.add(
'decide_next_object',
decide_next_object(),
transitions={'succeeded': 'go_to_object', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to poi where the object can stay
smach.StateMachine.add(
'go_to_object',
nav_to_poi(),
remapping={"nav_to_poi_name": "object_loc"},
transitions={'succeeded': 'object_detection', 'aborted': 'aborted'})
# Object Detection
smach.StateMachine.add(
'object_detection',
recognize_object(),
transitions={'succeeded': 'grasp_object', 'aborted': 'aborted'})
# Grasp Object
smach.StateMachine.add(
'grasp_object',
text_to_say("Grasping Object"),
transitions={'succeeded': 'go_to_delivery', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to the delivery place
smach.StateMachine.add(
'go_to_delivery',
nav_to_poi(),
remapping = {'nav_to_poi_name': 'delivery_loc'},
transitions={'succeeded': 'deliver_object', 'aborted': 'aborted',
'preempted': 'preempted'})
# Deliver object
smach.StateMachine.add(
'deliver_object',
text_to_say("Delivering Object"),
transitions={'succeeded': 'check_loop', 'aborted': 'aborted',
'preempted': 'preempted'})
# End of loop?
smach.StateMachine.add(
'check_loop',
checkLoop(),
transitions={'succeeded': 'decide_next_object', 'aborted': 'aborted',
'preempted': 'preempted', 'end':'succeeded'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
self.userdata.tts_wait_before_speaking=0
self.userdata.tts_text=None
self.userdata.tts_lang=None
self.userdata.nav_to_poi_name=None
self.userdata.standard_error='OK'
self.userdata.grammar_name="restaurant.gram"
# 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
# Must we say something to start? "I'm ready" or something
# Must we wait for the spoken order?
# Guide phase
smach.StateMachine.add(
'guide_phase',
restaurantGuide(),
transitions={'succeeded': 'start_restaurant', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to ordering location
smach.StateMachine.add(
'start_restaurant',
nav_to_poi('ordering'),
transitions={'succeeded': 'ask_order', 'aborted': 'aborted',
'preempted': 'preempted'})
# Ask for order
smach.StateMachine.add(
'ask_order',
RestaurantOrder(),
transitions={'succeeded': 'navigation_phase', 'aborted': 'aborted',
'preempted': 'preempted'})
# Navigation Phase
smach.StateMachine.add(
'navigation_phase',
RestaurantNavigation(),
transitions={'succeeded': 'leaving_ordering', 'aborted': 'aborted',
'preempted': 'preempted'})
# Leaving the arena
smach.StateMachine.add(
'leaving_ordering',
nav_to_poi('ordering'),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
def call_go_to(loc_name,world):
tosay = "I'm going to the "+str(loc_name)
speak = speaker(tosay,wait=False)
speak.execute()
rospy.logwarn('call_go_to '+ loc_name + ' from :')
#############################################################################
if SKILLS :
if (time.time()-TIME_INIT) > 270:
return "succeeded"
out = 'aborted'
tries = 0
while(out=='aborted' and tries<3):
tries = tries+1
if world.get_current_position() == loc_name:
out = 'succeeded'
else:
if loc_name == "exit":
sm = nav_to_poi(poi_name = "door_B")
out = sm.execute()
sm = nav_to_poi(poi_name = loc_name)
out = sm.execute()
if out=='aborted':
tosay = "I can't reach the " + loc_name + ". The door is closed. I'm going to inform"
speak = speaker(tosay)
speak.execute()
rospy.logwarn('FAIL IN REACHING ' + loc_name)
time.sleep(SLEEP_TIME)
sm = nav_to_poi(poi_name = 'referee')
out = sm.execute()
tosay = "I can't reach the " + loc_name + ". The door is closed. The sentence is from category 3"
speak = speaker(tosay)
speak.execute()
return "aborted"
else:
tosay = "I arrived to the " + loc_name
speak = speaker(tosay)
speak.execute()
#############################################################################
world.set_current_position(loc_name)
rospy.logwarn(world.get_current_position())
time.sleep(SLEEP_TIME)
return "succeeded"
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.nav_to_poi_name=''
# Prepare the poi for nav_to_poi
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': 'object_recognition', 'aborted': 'aborted',
'preempted': 'preempted'})
# Do object_recognition
smach.StateMachine.add(
'object_recognition',
DummyStateMachine(),
transitions={'succeeded': 'grasp_object', 'aborted': 'aborted',
'preempted': 'preempted'})
# Grasp the object
smach.StateMachine.add(
'grasp_object',
DummyStateMachine(),
transitions={'succeeded': 'go_second_location', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go the location - We need to go to the place to object category, so we assume that the
# object recognition will init the poi to the object must to go
smach.StateMachine.add(
'go_second_location',
nav_to_poi(),
transitions={'succeeded': 'release_object', 'aborted': 'aborted',
'preempted': 'preempted'})
# Release the object
smach.StateMachine.add(
'release_object',
DummyStateMachine(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
def call_go_to(loc_name, world):
tosay = "I'm going to the " + str(loc_name)
speak = speaker(tosay, wait=False)
speak.execute()
rospy.logwarn('call_go_to ' + loc_name + ' from :')
#############################################################################
if SKILLS:
if (time.time() - TIME_INIT) > 270:
return "succeeded"
out = 'aborted'
tries = 0
while (out == 'aborted' and tries < 3):
tries = tries + 1
if world.get_current_position() == loc_name:
out = 'succeeded'
else:
if loc_name == "exit":
sm = nav_to_poi(poi_name="door_B")
out = sm.execute()
sm = nav_to_poi(poi_name=loc_name)
out = sm.execute()
if out == 'aborted':
tosay = "I can't reach the " + loc_name + ". The door is closed. I'm going to inform"
speak = speaker(tosay)
speak.execute()
rospy.logwarn('FAIL IN REACHING ' + loc_name)
time.sleep(SLEEP_TIME)
sm = nav_to_poi(poi_name='referee')
out = sm.execute()
tosay = "I can't reach the " + loc_name + ". The door is closed. The sentence is from category 3"
speak = speaker(tosay)
speak.execute()
return "aborted"
else:
tosay = "I arrived to the " + loc_name
speak = speaker(tosay)
speak.execute()
#############################################################################
world.set_current_position(loc_name)
rospy.logwarn(world.get_current_position())
time.sleep(SLEEP_TIME)
return "succeeded"
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.nav_to_poi_name=''
# Go to pick and place
smach.StateMachine.add(
'go_pick_and_place',
nav_to_poi('init_pick_and_place'),
transitions={'succeeded': 'do_pick_and_place', 'aborted': 'aborted',
'preempted': 'preempted'})
# Do pick and place
smach.StateMachine.add(
'do_pick_and_place',
PickPlaceSM(),
transitions={'succeeded': 'go_avoid_that', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to avoid that
smach.StateMachine.add(
'go_avoid_that',
nav_to_poi('init_avoid_that'),
transitions={'succeeded': 'do_avoid_that', 'aborted': 'aborted',
'preempted': 'preempted'})
# Do avoid that
smach.StateMachine.add(
'do_avoid_that',
Avoid_That(),
transitions={'succeeded': 'go_what_did_you_say', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to what did you say
smach.StateMachine.add(
'go_what_did_you_say',
nav_to_poi('init_what_say'),
transitions={'succeeded': 'do_what_did_you_say', 'aborted': 'aborted',
'preempted': 'preempted'})
# Do what did you say
smach.StateMachine.add(
'do_what_did_you_say',
WhatSaySM(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
def call_guide_to(loc_name,world):
tosay = "Please follow me to the "+str(loc_name)
speak = speaker(tosay)
speak.execute()
rospy.logwarn('call_guide_to '+ loc_name)
# #############################################################################
if SKILLS :
if (time.time()-TIME_INIT) > 270:
return "succeeded"
out = 'aborted'
tries = 0
while(out=='aborted' and tries<3):
tries = tries+1
sm = nav_to_poi(poi_name = loc_name)
sm.execute()
if out=='aborted':
tosay = "I can't reach the " + loc_name + ". The door is closed. The sentence was from category 3"
speak = speaker(tosay)
speak.execute()
rospy.logwarn('FAIL IN REACHING ' + loc_name)
time.sleep(SLEEP_TIME)
return "aborted"
#############################################################################
world.set_current_position(loc_name)
time.sleep(SLEEP_TIME)
return "succeeded"
def __init__(self, poi_name=None):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['nav_to_poi_name'],
output_keys=['pose_current'])
with self:
smach.StateMachine.add('PrepareData',
prepareData(poi_name),
transitions={'succeeded':'Concurrence', 'aborted':'aborted'})
# Concurrence
sm_conc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
input_keys=['nav_to_poi_name'],
output_keys=['standard_error', 'pose_current'],
child_termination_cb = child_term_cb,
outcome_cb=out_cb)
with sm_conc:
smach.Concurrence.add('Go_to_poi', nav_to_poi())
smach.Concurrence.add('check_button', CheckButton())
smach.Concurrence.add('save_position', SavePosition())
smach.StateMachine.add(
'Concurrence',
sm_conc,
transitions={'succeeded': 'succeeded', 'aborted': 'Concurrence'})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error'])
with self:
# We prepare the information to go to the init door
smach.StateMachine.add('prepare_Avoid',
prepare_Avoid(),
transitions={
'succeeded': 'say_go_to_poi',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Announce going to a place
smach.StateMachine.add('say_go_to_poi',
text_to_say(text="I'm going to my destiny"),
transitions={'succeeded': 'go_to_poi'})
# Go to the POI
smach.StateMachine.add('go_to_poi',
nav_to_poi(),
transitions={
'succeeded': 'say_get_to_poi',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Announce arriving to a place
smach.StateMachine.add(
'say_get_to_poi',
text_to_say(text="I've arrived to my destiny"),
transitions={'succeeded': 'succeeded'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'], output_keys=['standard_error'])
with self:
# We prepare the information to go to the init door
smach.StateMachine.add(
'prepare_Avoid',
prepare_Avoid(),
transitions={'succeeded': 'say_go_to_poi', 'aborted': 'aborted',
'preempted': 'preempted'})
# Announce going to a place
smach.StateMachine.add(
'say_go_to_poi',
text_to_say(text="I'm going to the Avoid That area."),
transitions={'succeeded': 'go_to_poi'})
# Go to the POI
smach.StateMachine.add(
'go_to_poi',
nav_to_poi(),
transitions={'succeeded': 'say_get_to_poi', 'aborted': 'aborted',
'preempted': 'preempted'})
# Announce arriving to a place
smach.StateMachine.add(
'say_get_to_poi',
text_to_say(text="I've arrived to the destination for Avoid That. The test Avoid That has finished successfully. Thank you."),
transitions={'succeeded': 'succeeded'})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['object_name', 'name_face', 'list_orders', 'loop_iterations'],
output_keys=[])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
# Grasp Order
smach.StateMachine.add(
'grasp_order',
GraspOrder(),
transitions={'succeeded': 'go_to_party', 'aborted': 'aborted'})
# Go to the party room
smach.StateMachine.add(
'go_to_party',
nav_to_poi('living_room'),
transitions={'succeeded': 'deliver_order', 'aborted': 'go_to_party',
'preempted': 'preempted'})
# Deliver the order
smach.StateMachine.add(
'deliver_order',
DeliverOrder(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
def call_guide_to(loc_name, world):
tosay = "Please follow me to the " + str(loc_name)
speak = speaker(tosay)
speak.execute()
rospy.logwarn('call_guide_to ' + loc_name)
# #############################################################################
if SKILLS:
if (time.time() - TIME_INIT) > 270:
return "succeeded"
out = 'aborted'
tries = 0
while (out == 'aborted' and tries < 3):
tries = tries + 1
sm = nav_to_poi(poi_name=loc_name)
sm.execute()
if out == 'aborted':
tosay = "I can't reach the " + loc_name + ". The door is closed. The sentence was from category 3"
speak = speaker(tosay)
speak.execute()
rospy.logwarn('FAIL IN REACHING ' + loc_name)
time.sleep(SLEEP_TIME)
return "aborted"
#############################################################################
world.set_current_position(loc_name)
time.sleep(SLEEP_TIME)
return "succeeded"
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['object_name', 'name_face', 'list_orders', 'loop_iterations'],
output_keys=[])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
# Grasp Order
smach.StateMachine.add(
'grasp_order',
GraspOrder(),
transitions={'succeeded': 'go_to_party', 'aborted': 'aborted'})
# Go to the party room
smach.StateMachine.add(
'go_to_party',
nav_to_poi('party_room'),
transitions={'succeeded': 'deliver_order', 'aborted': 'go_to_party',
'preempted': 'preempted'})
# Deliver the order
smach.StateMachine.add(
'deliver_order',
DeliverOrder(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
output_keys=['list_orders'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.loop_iterations = 1
self.userdata.list_orders = []
self.userdata.delete_database=True # Ask Order
smach.StateMachine.add(
'ask_order',
AskOrder(),
transitions={'succeeded': 'process_info', 'aborted': 'aborted'})
# Process the info [name, object_name]
smach.StateMachine.add(
'process_info',
process_info(),
transitions={'succeeded': 'go_to_party', 'aborted': 'aborted'})
# Go to the party room
smach.StateMachine.add(
'go_to_party',
nav_to_poi('living_room'),
transitions={'succeeded': 'check_loop', 'aborted': 'check_loop',
'preempted': 'check_loop'})
# End of loop?
smach.StateMachine.add(
'check_loop',
checkLoop(),
transitions={'succeeded': 'ask_order', 'aborted': 'aborted',
'preempted': 'preempted', 'end':'succeeded'})
def main():
rospy.init_node('restaurant_listen_operator_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
smach.StateMachine.add('listen_operator',
ListenOperator(),
transitions={
'succeeded': 'rospi_go_to_poi',
'aborted': 'aborted'
})
sm.userdata.nav_to_poi_name = 'drinks'
smach.StateMachine.add('rospi_go_to_poi',
nav_to_poi(),
transitions={
'succeeded': 'listen_operator',
'aborted': 'aborted'
})
# This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
sis = smach_ros.IntrospectionServer('restaurant_listen_operator_Test', sm,
'/restaurant_guide_listen')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def __init__(self, nav_to_poi_name = None, tts_text = None):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['tts_text','nav_to_poi_name'],
output_keys=['standard_error'])
with self:
self.userdata.tts_lang='en_US'
self.userdata.tts_wait_before_speaking=0
self.userdata.standard_error='OK'
smach.StateMachine.add(
'INIT_VAR',
prepare_state(nav_to_poi_name, tts_text),
transitions={'succeeded': 'Concurrence_Say_Nav', 'aborted': 'aborted',
'preempted': 'preempted'})
sm_conc = smach.Concurrence(outcomes=['succeeded', 'preempted','aborted'],
default_outcome='succeeded',
input_keys=['tts_text',
'nav_to_poi_name',
'tts_wait_before_speaking',
'tts_lang'])
with sm_conc:
sm_conc.add('Say_conc',
text_to_say())
sm_conc.add('Nav_to_poi_conc',
nav_to_poi())
smach.StateMachine.add('Concurrence_Say_Nav',
sm_conc,
transitions={'succeeded':'succeeded',
'aborted':'aborted',
'preempted':'preempted'})
def main():
rospy.init_node('nav_to_poi_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.nav_to_poi_name = None
smach.StateMachine.add('nav_poi_test',
nav_to_poi("kitchen"),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
sis = smach_ros.IntrospectionServer('nav_poi_introspection', sm,
'/nav_poi_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def __init__(self):
smach.StateMachine.__init__(self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
output_keys=['poi_location'])
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
self.userdata.possible_pois = []
self.userdata.n_item = 0
smach.StateMachine.add(
'Prepare_Data',
Prepare_Data(),
transitions={'succeeded': 'Search_Emergency'})
smach.StateMachine.add(
'Search_Emergency',
Select_Possible_Poi(),
transitions={'succeeded':'Navigate_to_Room','finished_searching':'aborted'})
smach.StateMachine.add(
'Navigate_to_Room',
nav_to_poi(),
remapping={'nav_to_poi_name':'nav_to_poi_name_possible'},
transitions={'succeeded': 'Search_Wave', 'aborted': 'Navigate_to_Room', 'preempted': 'preempted'})
smach.StateMachine.add(
'Search_Wave',
Search_Wave_SM(),
transitions={'succeeded':'succeeded', 'preempted':'preempted', 'aborted':'aborted', 'end_searching':'Search_Emergency'})
def __init__(self, poi="", word=""):
smach.StateMachine.__init__(
self, outcomes=["succeeded", "preempted", "aborted"], input_keys=["nav_to_poi_name", "keyword_name"]
)
with self:
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_text = None
self.userdata.tts_lang = None
self.userdata.keyword_name = None
self.userdata.nav_to_poi_name = None
self.userdata.grammar_name = None
smach.StateMachine.add(
"PrepareData", prepareData(poi, word), transitions={"succeeded": "go_and_listen", "aborted": "aborted"}
)
sm = smach.Concurrence(
outcomes=["NAV", "LISTEN", "preempted"],
default_outcome="NAV",
input_keys=["nav_to_poi_name", "keyword_name"],
child_termination_cb=child_term_cb,
outcome_cb=out_cb_follow,
output_keys=[],
)
with sm:
smach.Concurrence.add("go_to_poi", nav_to_poi())
smach.Concurrence.add("listen_word", ListenWordSM_Concurrent(word))
smach.StateMachine.add(
"go_and_listen", sm, transitions={"NAV": "succeeded", "LISTEN": "aborted", "preempted": "succeeded"}
)
def __init__(self):
smach.StateMachine.__init__(self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
output_keys=['emergency_poi_name', 'wave_position', 'wave_yaw_degree'])
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
self.userdata.possible_pois = []
self.userdata.n_item = 0
smach.StateMachine.add(
'Prepare_Data',
Prepare_Data(),
transitions={'succeeded': 'Search_Emergency'})
smach.StateMachine.add(
'Search_Emergency',
Select_Possible_Poi(),
transitions={'succeeded':'Say_Go_to_Room','finished_searching':'aborted'})
smach.StateMachine.add(
'Say_Go_to_Room',
text_to_say("Now I am going to the next room"),
transitions={'succeeded':'Navigate_to_Room','aborted':'aborted'})
smach.StateMachine.add(
'Navigate_to_Room',
nav_to_poi(),
remapping={'nav_to_poi_name':'nav_to_poi_name_possible'},
transitions={'succeeded': 'Search_Wave', 'aborted': 'Navigate_to_Room', 'preempted': 'preempted'})
smach.StateMachine.add(
'Search_Wave',
Search_Wave_SM(head_position='down',text_for_wave_searching='Where are you? I am trying to find and help you.'),
transitions={'succeeded':'Normal_head_Out', 'preempted':'preempted',
'aborted':'aborted',
'end_searching':'Normal_head'})
smach.StateMachine.add(
'Normal_head_Out',
move_head_form(head_left_right='center', head_up_down='normal'),
transitions={'succeeded':'Prepare_Output_Search', 'preempted':'Prepare_Output_Search',
'aborted':'Prepare_Output_Search'})
smach.StateMachine.add(
'Normal_head',
move_head_form(head_left_right='center', head_up_down='normal'),
transitions={'succeeded':'Search_Emergency', 'preempted':'Search_Emergency',
'aborted':'Search_Emergency'})
smach.StateMachine.add(
'Prepare_Output_Search',
Prepare_output_search(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
self.userdata.tts_wait_before_speaking=0
self.userdata.tts_text=None
self.userdata.tts_lang=None
self.userdata.nav_to_poi_name=None
self.userdata.standard_error='OK'
self.userdata.grammar_name="restaurant.gram"
# 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
# Must we say something to start? "I'm ready" or something
# Must we wait for the spoken order?
# Guide phase
smach.StateMachine.add(
'guide_phase',
restaurantGuide(),
transitions={'succeeded': 'start_restaurant', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to ordering location, i thinc it will not be necessary
smach.StateMachine.add(
'start_restaurant',
nav_to_poi_say(tts="i am going to the ordering poi",poi_name='ordering'),
transitions={'succeeded': 'ask_order', 'aborted': 'ask_order',
'preempted': 'preempted'})
# Ask for order
smach.StateMachine.add(
'ask_order',
RestaurantOrder(),
transitions={'succeeded': 'navigation_phase', 'aborted': 'aborted',
'preempted': 'preempted'})
# Navigation Phase
smach.StateMachine.add(
'navigation_phase',
RestaurantNavigation(),
transitions={'succeeded': 'leaving_ordering', 'aborted': 'aborted',
'preempted': 'preempted'})
# Leaving the arena
smach.StateMachine.add(
'leaving_ordering',
nav_to_poi('ordering'),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['name', 'nav_to_poi_name', 'face'],
output_keys=['face', 'standard_error'])
with self:
self.userdata.num_iterations = 0
self.userdata.face = None
self.userdata.wait_time = 15
# We define the different points
smach.StateMachine.add('prepare_poi',
prepare_poi(),
transitions={
'succeeded': 'Concurrence',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Concurrence
sm_conc = smach.Concurrence(
outcomes=['succeeded', 'aborted', 'preempted', 'endTime'],
default_outcome='succeeded',
input_keys=['name', 'nav_to_poi_name', 'face', 'wait_time'],
output_keys=['face', 'standard_error'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# Go around the room
smach.Concurrence.add('walk_to_poi', nav_to_poi())
# Move head
smach.Concurrence.add('TimeOut', TimeOut())
# Search for face
smach.Concurrence.add('find_faces',
recognize_face_concurrent())
smach.StateMachine.add('Concurrence',
sm_conc,
transitions={
'succeeded': 'succeeded',
'aborted': 'prepare_poi',
'endTime': 'aborted'
})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error'])
with self:
# Home position
smach.StateMachine.add(
'home_position_init',
play_motion_sm('home'),
transitions={
'succeeded': 'prepare_Avoid',
'aborted':
'home_position_init', #TODO: Change aborted to try again
'preempted': 'preempted'
})
# We prepare the information to go to the init door
smach.StateMachine.add('prepare_Avoid',
prepare_Avoid(),
transitions={
'succeeded': 'say_go_to_poi',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Announce going to a place
smach.StateMachine.add(
'say_go_to_poi',
text_to_say(text="I'm going to the Avoid That location."),
transitions={'succeeded': 'go_to_poi'})
# Go to the POI
smach.StateMachine.add('go_to_poi',
nav_to_poi(),
transitions={
'succeeded': 'say_get_to_poi',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Announce arriving to a place
smach.StateMachine.add(
'say_get_to_poi',
text_to_say(
text="I've arrived to the final destination for Avoid That."
),
transitions={'succeeded': 'succeeded'})
def __init__(self):
smach.StateMachine.__init__(self, ["succeeded", "preempted", "aborted"], output_keys=["standard_error"])
with self:
# We prepare the information to go to the init door
smach.StateMachine.add(
"prepare_Avoid",
prepare_Avoid(),
transitions={"succeeded": "go_to_poi", "aborted": "aborted", "preempted": "preempted"},
)
# Go to the POI
smach.StateMachine.add(
"go_to_poi",
nav_to_poi(),
transitions={"succeeded": "succeeded", "aborted": "aborted", "preempted": "preempted"},
)
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['list_orders'],
output_keys=['list_orders'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.loop_iterations = 1
self.userdata.object_name = []
self.userdata.list_orders = [['hannah', 'red bull'], ['rebecca', 'water'], ['andrew', 'Cocacola']]
# Go to the storage_room
smach.StateMachine.add(
'go_to_storage',
nav_to_poi("storage_room"),
transitions={'succeeded': 'prepare_recognize', 'aborted': 'prepare_recognize',
'preempted': 'preempted'})
# Prepare next order
smach.StateMachine.add(
'prepare_recognize',
prepare_recognize(),
transitions={'succeeded': 'execute_order', 'aborted': 'aborted',
'preempted': 'preempted'})
# Execute Order
smach.StateMachine.add(
'execute_order',
ExecuteOrder(),
transitions={'succeeded': 'check_loop', 'aborted': 'aborted'})
# End of loop?
smach.StateMachine.add(
'check_loop',
checkLoop(),
transitions={'succeeded': 'play_motion_state', 'aborted': 'aborted',
'preempted': 'preempted', 'end':'succeeded'})
smach.StateMachine.add(
'play_motion_state',
play_motion_sm('home', skip_planning=True),
transitions={'succeeded': 'go_to_storage',
'preempted':'go_to_storage',
'aborted':'play_motion_state'})
def __init__(self, object_name = None):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['object_name'],
output_keys=['standard_error', 'objectd'])
with self:
smach.StateMachine.add('PrepareData',
prepareData(object_name),
transitions={'succeeded':'get_object_info_sm', 'aborted':'aborted'})
# Obtain the location where the object can stay
smach.StateMachine.add('get_object_info_sm',
GetObjectInfoSM(),
transitions={'succeeded': 'say_go_to_poi',
'aborted': 'aborted',
'preempted': 'preempted'})
# say that it goes to the poi
smach.StateMachine.add(
'say_go_to_poi',
text_to_say("I'm going to take the object"),
transitions={'succeeded': 'go_to_object', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to poi where the object can stay
smach.StateMachine.add(
'go_to_object',
nav_to_poi(),
remapping={"nav_to_poi_name": "object_location"},
transitions={'succeeded': 'say_start_recognition', 'aborted': 'aborted'})
# Say start recognition
smach.StateMachine.add(
'say_start_recognition',
text_to_say("I'm going to start object recognition"),
transitions={'succeeded': 'object_detection', 'aborted': 'object_detection',
'preempted': 'preempted'})
# Object Detection
smach.StateMachine.add(
'object_detection',
recognize_object(),
transitions={'succeeded': 'succeeded', 'aborted': 'get_object_info_sm'})
def __init__(self, poi='', word=''):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['nav_to_poi_name', 'keyword_name'])
with self:
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_text = None
self.userdata.tts_lang = None
self.userdata.keyword_name = None
self.userdata.nav_to_poi_name = None
self.userdata.grammar_name = None
smach.StateMachine.add('PrepareData',
prepareData(poi, word),
transitions={
'succeeded': 'go_and_listen',
'aborted': 'aborted'
})
sm = smach.Concurrence(
outcomes=['NAV', 'LISTEN', 'preempted'],
default_outcome='NAV',
input_keys=["nav_to_poi_name", 'keyword_name'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb_follow,
output_keys=[])
with sm:
smach.Concurrence.add('go_to_poi', nav_to_poi())
smach.Concurrence.add('listen_word',
ListenWordSM_Concurrent(word))
smach.StateMachine.add('go_and_listen',
sm,
transitions={
'NAV': 'succeeded',
'LISTEN': 'aborted',
'preempted': 'succeeded'
})
def main():
rospy.init_node('restaurant_listen_operator_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
smach.StateMachine.add('ask_order',
RestaurantOrder(),
transitions={
'succeeded': 'navigation_phase',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Navigation Phase
smach.StateMachine.add('navigation_phase',
RestaurantNavigation(),
transitions={
'succeeded': 'leaving_ordering',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Leaving the arena
smach.StateMachine.add('leaving_ordering',
nav_to_poi('ordering'),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
# This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
sis = smach_ros.IntrospectionServer('restaurant_listen_operator_Test', sm,
'/restaurant_guide_listen')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
output_keys=['poi_location'])
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
self.userdata.possible_pois = []
self.userdata.n_item = 0
smach.StateMachine.add(
'Prepare_Data',
Prepare_Data(),
transitions={'succeeded': 'Search_Emergency'})
smach.StateMachine.add('Search_Emergency',
Select_Possible_Poi(),
transitions={
'succeeded': 'Navigate_to_Room',
'finished_searching': 'aborted'
})
smach.StateMachine.add(
'Navigate_to_Room',
nav_to_poi(),
remapping={'nav_to_poi_name': 'nav_to_poi_name_possible'},
transitions={
'succeeded': 'Search_Wave',
'aborted': 'Navigate_to_Room',
'preempted': 'preempted'
})
smach.StateMachine.add('Search_Wave',
Search_Wave_SM(),
transitions={
'succeeded': 'succeeded',
'preempted': 'preempted',
'aborted': 'aborted',
'end_searching': 'Search_Emergency'
})
def main():
rospy.init_node('cocktail_party')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
STATES = [CocktailPartySM()]
STATE_NAMES = ["CocktailPartySM"]
# We have 10 minuts -> 600 sec
# smach.StateMachine.add(
# "Cocktail_test",
# ConcurrenceTime(states=STATES, state_names=STATE_NAMES, timeout=570),
# transitions={'succeeded': 'leaving_arena', 'aborted': "Say_timeout"})
smach.StateMachine.add(
"Cocktail_test",
CocktailPartySM(),
transitions={'succeeded': 'leaving_arena', 'aborted': "Say_timeout"})
# Say TimeOut
smach.StateMachine.add(
'Say_timeout',
text_to_say("My time to do the test is over. I going to leave the arena", wait=False),
transitions={'succeeded': 'leaving_arena', 'aborted': 'leaving_arena'})
# Leaving the arena
smach.StateMachine.add(
'leaving_arena',
nav_to_poi('leave_arena'),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})
# This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
sis = smach_ros.IntrospectionServer(
'cocktail_party', sm, '/CP_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['name', 'nav_to_poi_name', 'face'],
output_keys=['face', 'standard_error'])
with self:
self.userdata.num_iterations = 0
self.userdata.face = None
self.userdata.wait_time = 15
# We define the different points
smach.StateMachine.add(
'prepare_poi',
prepare_poi(),
transitions={'succeeded': 'Concurrence', 'aborted': 'aborted',
'preempted': 'preempted'})
# Concurrence
sm_conc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted', 'endTime'],
default_outcome='succeeded',
input_keys=['name', 'nav_to_poi_name', 'face', 'wait_time'],
output_keys=['face', 'standard_error'],
child_termination_cb = child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# Go around the room
smach.Concurrence.add('walk_to_poi', nav_to_poi())
# Move head
smach.Concurrence.add('TimeOut', TimeOut())
# Search for face
smach.Concurrence.add('find_faces', recognize_face_concurrent())
smach.StateMachine.add('Concurrence', sm_conc,
transitions={'succeeded':'succeeded', 'aborted':'prepare_poi', 'endTime': 'aborted'})
def main():
rospy.init_node('restaurant_listen_operator_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
smach.StateMachine.add(
'ask_order',
RestaurantOrder(),
transitions={'succeeded': 'navigation_phase', 'aborted': 'aborted',
'preempted': 'preempted'})
# Navigation Phase
smach.StateMachine.add(
'navigation_phase',
RestaurantNavigation(),
transitions={'succeeded': 'leaving_ordering', 'aborted': 'aborted',
'preempted': 'preempted'})
# Leaving the arena
smach.StateMachine.add(
'leaving_ordering',
nav_to_poi('ordering'),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
# This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
sis = smach_ros.IntrospectionServer(
'restaurant_listen_operator_Test', sm, '/restaurant_guide_listen')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error'])
with self:
# We prepare the information to go to the init door
smach.StateMachine.add('prepare_Avoid',
prepare_Avoid(),
transitions={
'succeeded': 'go_to_poi',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Go to the POI
smach.StateMachine.add('go_to_poi',
nav_to_poi(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
def main():
rospy.init_node('nav_to_poi_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.nav_to_poi_name = None
smach.StateMachine.add(
'nav_poi_test',
nav_to_poi("point_room_one"),
transitions={'succeeded': 'succeeded','aborted' : 'aborted'})
# This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
sis = smach_ros.IntrospectionServer(
'nav_poi_introspection', sm, '/nav_poi_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node("restaurant_listen_operator_test")
sm = smach.StateMachine(outcomes=["succeeded", "preempted", "aborted"])
with sm:
smach.StateMachine.add(
"listen_operator", ListenOperator(), transitions={"succeeded": "rospi_go_to_poi", "aborted": "aborted"}
)
sm.userdata.nav_to_poi_name = "drinks"
smach.StateMachine.add(
"rospi_go_to_poi", nav_to_poi(), transitions={"succeeded": "listen_operator", "aborted": "aborted"}
)
# This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
sis = smach_ros.IntrospectionServer("restaurant_listen_operator_Test", sm, "/restaurant_guide_listen")
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'], output_keys=['standard_error'])
with self:
# Home position
smach.StateMachine.add(
'home_position_init',
play_motion_sm('home'),
transitions={'succeeded': 'prepare_Avoid', 'aborted': 'home_position_init', #TODO: Change aborted to try again
'preempted': 'preempted'})
# We prepare the information to go to the init door
smach.StateMachine.add(
'prepare_Avoid',
prepare_Avoid(),
transitions={'succeeded': 'say_go_to_poi', 'aborted': 'aborted',
'preempted': 'preempted'})
# Announce going to a place
smach.StateMachine.add(
'say_go_to_poi',
text_to_say(text="I'm going to the Avoid That location."),
transitions={'succeeded': 'go_to_poi'})
# Go to the POI
smach.StateMachine.add(
'go_to_poi',
nav_to_poi(),
transitions={'succeeded': 'say_get_to_poi', 'aborted': 'aborted',
'preempted': 'preempted'})
# Announce arriving to a place
smach.StateMachine.add(
'say_get_to_poi',
text_to_say(text="I've arrived to the final destination for Avoid That."),
transitions={'succeeded': 'succeeded'})
def __init__(self, poi_name=None):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['nav_to_poi_name'],
output_keys=['pose_current'])
with self:
smach.StateMachine.add('PrepareData',
prepareData(poi_name),
transitions={
'succeeded': 'Concurrence',
'aborted': 'aborted'
})
# Concurrence
sm_conc = smach.Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
input_keys=['nav_to_poi_name'],
output_keys=['standard_error', 'pose_current'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm_conc:
smach.Concurrence.add('Go_to_poi', nav_to_poi())
smach.Concurrence.add('check_button', CheckButton())
smach.Concurrence.add('save_position', SavePosition())
smach.StateMachine.add('Concurrence',
sm_conc,
transitions={
'succeeded': 'succeeded',
'aborted': 'Concurrence'
})
def __init__(self,poi_name=None,time_out=10):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['name', 'nav_to_poi_name', 'face','face_frame'],
output_keys=['face', 'standard_error', 'face_frame'])
self.poi_name=poi_name
self.time_out=time_out
with self:
self.userdata.num_iterations = 0
self.userdata.face = None
self.userdata.wait_time = 5
self.userdata.nav_to_poi_name=None
self.userdata.face_frame=None
# We define the different points
smach.StateMachine.add(
'prepare_poi',
prepare_poi(self.poi_name),
transitions={'poi': 'go_poi', 'no_poi': 'Concurrence'})
smach.StateMachine.add(
'Say_Searching',
text_to_say('Right Now I am looking for you.',wait=True),
transitions={'succeeded': 'go_poi', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'go_poi',
nav_to_poi(poi_name),
transitions={'succeeded': 'Concurrence', 'aborted': 'Concurrence', #TODO
'preempted': 'preempted'})
# Concurrence
sm_conc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted', 'endTime'],
default_outcome='succeeded',
input_keys=['name', 'nav_to_poi_name', 'face', 'wait_time'],
output_keys=['face', 'standard_error', 'face_frame'],
child_termination_cb = child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# Go around the room
# smach.Concurrence.add('walk_to_poi', nav_to_poi())
#smach.Concurrence.add('turn', turn_infinit('left'))
# Move head
smach.Concurrence.add('TimeOut', TimeOut(self.time_out))
# Search for face
smach.Concurrence.add('find_faces', recognize_face_concurrent())
smach.Concurrence.add('say_search_faces', say_searching_faces())
smach.StateMachine.add('Concurrence', sm_conc,
transitions={'succeeded':'DEFAULT_POSITION',
'aborted':'Say_Finish',
'endTime': 'Say_Finish',
'preempted':'Say_Finish'})
smach.StateMachine.add(
'Say_Finish',
text_to_say('I have finish with no exit the search',wait=False),
transitions={'succeeded': 'DEFAULT_POSITION_NO', 'aborted': 'DEFAULT_POSITION_NO',
'preempted': 'preempted'})
smach.StateMachine.add('DEFAULT_POSITION_NO',
move_head_form("center","up"),
transitions={'succeeded': 'aborted','aborted':'aborted'})
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center","up"),
transitions={'succeeded': 'succeeded','aborted':'succeeded'})
def __init__(self, object_name=None):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['object_name'],
output_keys=[
'standard_error', 'object_position', 'object_detected_name'
])
with self:
self.userdata.number_search_object = 0
#self.userdata.number_search_object_POI = 0
self.userdata.nav_first_pass = True
smach.StateMachine.add('PrepareData',
prepareData(object_name),
transitions={
'succeeded': 'get_object_info_sm',
'aborted': 'aborted'
})
# Obtain the location where the object can stay
smach.StateMachine.add('get_object_info_sm',
GetObjectInfoSM(),
transitions={
'succeeded': 'say_go_to_poi',
'aborted': 'aborted',
'preempted': 'preempted'
})
# say that it goes to the POI indicated in the previous SM
smach.StateMachine.add('say_go_to_poi',
text_to_say("I'm going to take the object",
wait=False),
transitions={
'succeeded': 'go_to_object',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Go to poi where the object stays
smach.StateMachine.add(
'go_to_object',
nav_to_poi(),
remapping={"nav_to_poi_name": "object_location"},
transitions={
'succeeded': 'say_start_recognition',
'aborted': 'aborted'
})
# Say start recognition
smach.StateMachine.add('say_start_recognition',
text_to_say(
"I'm going to start object recognition",
wait=False),
transitions={
'succeeded': 'object_detection',
'aborted': 'object_detection',
'preempted': 'preempted'
})
# Object Detection
smach.StateMachine.add(
'object_detection',
recognize_object(),
#transitions={'succeeded': 'analyze_object_data', 'aborted': 'get_object_info_sm'})
transitions={
'succeeded': 'analyze_object_data',
'aborted': 'fail_object_detection'
})
smach.StateMachine.add('analyze_object_data',
analyze_object_data(),
transitions={
'succeeded': 'say_found_object',
'aborted': 'object_detection'
})
# Say found the object
#TODO: Tell x, y, z of the object
smach.StateMachine.add('say_found_object',
text_to_say("I found the object"),
transitions={
'succeeded': 'succeeded',
'aborted': 'succeeded',
'preempted': 'succeeded'
})
smach.StateMachine.add('fail_object_detection',
fail_object_detection_check(),
transitions={
'aborted': 'pick_anything_prepare',
'succeeded': 'say_re_detect'
})
smach.StateMachine.add(
'say_re_detect',
text_to_say(
'I am trying again to detect the object you asked'),
transitions={
'succeeded': 'object_detection',
'aborted': 'aborted'
})
smach.StateMachine.add('fail_object_detection_poi',
fail_object_detection_check_poi(),
transitions={
'succeeded': 'say_re_go',
'aborted': 'aborted'
})
smach.StateMachine.add(
'say_re_go',
text_to_say(
"I am going to another place to see if the object I am looking is there"
),
transitions={
'succeeded': 'Prepare_next_poi',
'aborted': 'aborted'
})
smach.StateMachine.add('Prepare_next_poi',
prepare_next_poi(),
transitions={
'succeeded': 'go_to_object',
'aborted': 'pick_anything_prepare'
})
smach.StateMachine.add('pick_anything_prepare',
pick_anything_prepare(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
output_keys=[
'emergency_poi_name', 'wave_position', 'wave_yaw_degree'
])
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
self.userdata.possible_pois = []
self.userdata.n_item = 0
smach.StateMachine.add(
'Prepare_Data',
Prepare_Data(),
transitions={'succeeded': 'Search_Emergency'})
smach.StateMachine.add('Search_Emergency',
Select_Possible_Poi(),
transitions={
'succeeded': 'Say_Go_to_Room',
'finished_searching': 'aborted'
})
smach.StateMachine.add(
'Say_Go_to_Room',
text_to_say("Now I am going to the next room"),
transitions={
'succeeded': 'Navigate_to_Room',
'aborted': 'aborted'
})
smach.StateMachine.add(
'Navigate_to_Room',
nav_to_poi(),
remapping={'nav_to_poi_name': 'nav_to_poi_name_possible'},
transitions={
'succeeded': 'Search_Wave',
'aborted': 'Search_Wave',
'preempted': 'preempted'
})
smach.StateMachine.add(
'Search_Wave',
Search_Wave_SM(
head_position='down',
text_for_wave_searching=
'Where are you? I am trying to find and help you.'),
transitions={
'succeeded': 'Normal_head_Out',
'preempted': 'preempted',
'aborted': 'aborted',
'end_searching': 'Normal_head'
})
smach.StateMachine.add('Normal_head_Out',
move_head_form(head_left_right='center',
head_up_down='normal'),
transitions={
'succeeded': 'Prepare_Output_Search',
'preempted': 'Prepare_Output_Search',
'aborted': 'Prepare_Output_Search'
})
smach.StateMachine.add('Normal_head',
move_head_form(head_left_right='center',
head_up_down='normal'),
transitions={
'succeeded': 'Search_Emergency',
'preempted': 'Search_Emergency',
'aborted': 'Search_Emergency'
})
smach.StateMachine.add('Prepare_Output_Search',
Prepare_output_search(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.nav_to_poi_name = ''
# Prepare the poi for nav_to_poi
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': 'object_recognition',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Do object_recognition
smach.StateMachine.add('object_recognition',
DummyStateMachine(),
transitions={
'succeeded': 'grasp_object',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Grasp the object
smach.StateMachine.add('grasp_object',
DummyStateMachine(),
transitions={
'succeeded': 'go_second_location',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Go the location - We need to go to the place to object category, so we assume that the
# object recognition will init the poi to the object must to go
smach.StateMachine.add('go_second_location',
nav_to_poi(),
transitions={
'succeeded': 'release_object',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Release the object
smach.StateMachine.add('release_object',
DummyStateMachine(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
input_keys=['person_location', 'person_location_coord'])
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
smach.StateMachine.add(
'Ask_Question',
text_to_say(text='What would you like me to bring?'),
transitions={'succeeded':'Listen_Question', 'aborted': 'Ask_Question', 'preempted':'Listen_Question'})
# Ask for the object to bring to the person
# Output:
# - string 'userSaid'
# - actiontag[] 'asr_userSaid_tags'
# TODO: grammar for the Emergency Situation -- Get_Person_Desired_Object
smach.StateMachine.add(
'Listen_Question',
ListenToSM(grammar='robocup/emergency'),
transitions={'succeeded':'Process_Tags', 'aborted':'Ask_Question', 'preempted':'Ask_Question'})
# Get the output from AskQuestionSM, process it, and search in the yaml file for the location of the object asked
# Input keys: actiontag[] 'asr_userSaid_tags'
# Output keys: object
smach.StateMachine.add(
'Process_Tags',
Process_Tags(),
transitions={'succeeded':'Say_go_Kitchen', 'aborted':'Ask_Question', 'aborted':'Ask_Question'})
smach.StateMachine.add(
'Say_go_Kitchen',
text_to_say('I am Going to the Kitchen for an object, Stay Here until I give you the object'),
transitions={'succeeded':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place'})
smach.StateMachine.add(
'Go_To_Object_Place',
nav_to_poi('kitchen'),
transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'Grasp_fail_Ask_Person'})
self.userdata.time_grasp = 0.0
smach.StateMachine.add('Grasping_with_timeout',
grasping_with_timeout(),
transitions={'succeeded':'Prepare_Go_To_Person', 'time_out':'Grasp_fail_Ask_Person'})
# sm_conc = smach.Concurrence(outcomes=['succeeded', 'time_out'],
# default_outcome='succeeded',
# input_keys=['object_to_grasp, time_grasp'],
# child_termination_cb = child_term_cb,
# outcome_cb = out_cb)
#
# with sm_conc:
# sm_conc.add(
# 'Find_and_grab_object',
# #Find_and_grab_object(),
# DummyStateMachine())
# sm_conc.add(
# 'Time_State',
# Time_State())
#
# smach.StateMachine.add('GRASP_CONCURRENCE',
# sm_conc,
# transitions={'succeeded':'Prepare_Go_To_Person',
# 'time_out':'Grasp_fail_Ask_Person'})
#Find Object + Grab Object SM
# smach.StateMachine.add(
# 'Find_and_grab_object',
# #Find_and_grab_object(),
# DummyStateMachine(),
# transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'Grasp_fail_Ask_Person'})
smach.StateMachine.add(
'Grasp_fail_Ask_Person',
ask_give_object_grasping(),
transitions={'succeeded':'Rest_arm', 'aborted':'Rest_arm', 'preempted':'Rest_arm'})
smach.StateMachine.add(
'Rest_arm',
play_motion_sm('rest_object_right'),
transitions={'succeeded':'Prepare_Go_To_Person', 'aborted':'Prepare_Go_To_Person', 'preempted':'Prepare_Go_To_Person'})
#Go to person
smach.StateMachine.add(
'Prepare_Go_To_Person',
prepare_poi_person_emergency(),
transitions={'succeeded':'Go_To_Person', 'aborted':'Go_To_Person', 'preempted':'Go_To_Person'})
#TODO: POI For Person in Emergency -- From SearchPeople SM -
smach.StateMachine.add(
'Go_To_Person',
#DummyStateMachine(),
#nav_to_poi(),
nav_to_coord('/map'),
transitions={'succeeded':'Say_Give_Object', 'aborted':'Say_Give_Object', 'preempted':'Say_Give_Object'})
smach.StateMachine.add(
'Say_Give_Object',
text_to_say('I am going to give you the Object you want.'),
transitions={'succeeded':'Give_object_arm', 'aborted':'Give_object_arm', 'preempted':'Give_object_arm'})
smach.StateMachine.add(
'Give_object_arm',
play_motion_sm('give_object_right'),
transitions={'succeeded':'Give_Object', 'aborted':'Give_Object', 'preempted':'Give_Object'})
#Give the grabbed object to the person
smach.StateMachine.add(
'Give_Object',
#DummyStateMachine(),
move_hands_form(hand_pose_name='pre_grasp', hand_side='right'),
transitions={'succeeded':'Give_Object_2', 'aborted':'Give_Object', 'preempted':'Give_Object'})
smach.StateMachine.add(
'Give_Object_2',
#DummyStateMachine(),
move_hands_form(hand_pose_name='full_open', hand_side='right'),
transitions={'succeeded':'Say_Rescue_stay', 'aborted':'Give_Object_2', 'preempted':'Give_Object_2'})
smach.StateMachine.add(
'Say_Rescue_stay',
text_to_say('Please Stay here I am going to call for the Ambulance'),
transitions={'succeeded':'succeeded', 'aborted':'aborted', 'aborted':'preempted'})
def __init__(self, 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'])
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 = ''
# Say Start basic Functionalities
smach.StateMachine.add(
'say_start_basic_functionalities',
text_to_say("I'm ready to start Basic Functionalities"),
transitions={
'succeeded': 'say_going_pick_place',
'aborted': 'say_going_pick_place'
})
# Say Go Pick and Place
smach.StateMachine.add(
'say_going_pick_place',
text_to_say("I'm going to the Pick and Place poi"),
transitions={
'succeeded': 'go_pick_and_place',
'aborted': 'go_pick_and_place'
})
# Go to pick and place
smach.StateMachine.add('go_pick_and_place',
nav_to_poi('init_pick_and_place'),
transitions={
'succeeded': 'do_pick_and_place',
'aborted': 'say_going_pick_place',
'preempted': 'preempted'
})
# Do pick and place
smach.StateMachine.add('do_pick_and_place',
PickPlaceSM(),
transitions={
'succeeded': 'say_going_avoid',
'aborted': 'say_going_avoid',
'preempted': 'preempted'
})
# Say Go Avoid that
smach.StateMachine.add(
'say_going_avoid',
text_to_say("I'm going to the Avoid that poi"),
transitions={
'succeeded': 'go_avoid_that',
'aborted': 'go_avoid_that'
})
# Go to avoid that
smach.StateMachine.add('go_avoid_that',
nav_to_poi('init_avoid_that'),
transitions={
'succeeded': 'do_avoid_that',
'aborted': 'say_going_avoid',
'preempted': 'preempted'
})
# Do avoid that
smach.StateMachine.add('do_avoid_that',
Avoid_That(),
transitions={
'succeeded': 'go_what_did_you_say',
'aborted': 'go_what_did_you_say',
'preempted': 'preempted'
})
# Say Go What did you say
smach.StateMachine.add(
'say_going_what_say',
text_to_say("I'm going to the What did you say poi"),
transitions={
'succeeded': 'go_pick_and_place',
'aborted': 'go_pick_and_place'
})
# Go to what did you say
smach.StateMachine.add('go_what_did_you_say',
nav_to_poi('init_what_say'),
transitions={
'succeeded': 'do_what_did_you_say',
'aborted': 'say_going_what_say',
'preempted': 'preempted'
})
# Do what did you say
smach.StateMachine.add('do_what_did_you_say',
WhatSaySM(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['name', 'nav_to_poi_name', 'face', 'face_frame'],
output_keys=['face', 'standard_error', 'face_frame'])
with self:
self.userdata.num_iterations = 0
self.userdata.face = None
self.userdata.wait_time = 5
# We define the different points
smach.StateMachine.add('prepare_poi',
prepare_poi(),
transitions={
'succeeded': 'Say_Searching',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add(
'Say_Searching',
text_to_say('Right Now I am looking for you.'),
transitions={
'succeeded': 'Concurrence',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Concurrence
sm_conc = smach.Concurrence(
outcomes=['succeeded', 'aborted', 'preempted', 'endTime'],
default_outcome='succeeded',
input_keys=['name', 'nav_to_poi_name', 'face', 'wait_time'],
output_keys=['face', 'standard_error', 'face_frame'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# Go around the room
smach.Concurrence.add('walk_to_poi', nav_to_poi())
# Move head
smach.Concurrence.add('TimeOut', TimeOut())
# Search for face
smach.Concurrence.add('find_faces',
recognize_face_concurrent())
smach.Concurrence.add('say_search_faces',
say_searching_faces())
smach.StateMachine.add('Concurrence',
sm_conc,
transitions={
'succeeded': 'succeeded',
'aborted': 'Say_Changing_Poi',
'endTime': 'Say_Changing_Poi',
'preempted': 'Say_Changing_Poi'
})
smach.StateMachine.add(
'Say_Changing_Poi',
text_to_say(
'I am going to change my position so I can search for faces'
),
transitions={
'succeeded': 'prepare_poi',
'aborted': 'aborted',
'preempted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.nav_to_poi_name=''
# 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):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['object_array'])
pose_place = PoseStamped()
pose_place.header.frame_id = '/base_link'
pose_place.pose.position.x = 0.90
pose_place.pose.position.y = 0.0
pose_place.pose.position.z = 1.20
pose_place.pose.orientation.w = 1.0
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 = None
self.userdata.standard_error = 'OK'
self.userdata.object_name = ''
self.userdata.object_index = 0
self.userdata.object_name = None
self.userdata.time_out_grasp = TIME_OUT_GRASP
# Process order
smach.StateMachine.add('decide_next_object',
decide_next_object(),
remapping={"object_loc": "nav_to_poi_name"},
transitions={
'succeeded': 'say_im_going',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Deliver object
smach.StateMachine.add('say_im_going',
text_to_say(),
transitions={
'succeeded': 'go_to_object',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Go to poi where the object can stay
smach.StateMachine.add('go_to_object',
nav_to_poi(),
transitions={
'succeeded': 'say_grasp_object',
'aborted': 'go_to_object'
})
# Grasp Object
smach.StateMachine.add(
'say_grasp_object',
text_to_say("I'm going to recognize and grasp the object"),
transitions={
'succeeded': 'recognize_object_and_pick',
'aborted': 'aborted',
'preempted': 'preempted'
})
# smach.StateMachine.add(
# 'Search_and_grasp_Object',
# object_detection_and_grasping_sm(),
# transitions={'succeeded':'prepare_delivery_goal',
# 'fail_object_detection':'say_object_not_find',
# 'fail_object_grasping':'say_not_grasp',
# 'aborted':'aborted',
# 'preempted':'preempted'},
# remapping = {'object_name':'object_to_grasp'})
# Recognize and pick object if found
# Grasp the object
# Grasp the object
smach.StateMachine.add('grasp_object',
pick_object_sm(),
transitions={
'succeeded': 'succeeded',
'aborted': 'say_not_grasp',
'preempted': 'preempted'
})
smach.StateMachine.add('recognize_object_and_pick',
RecObjectAndPick(),
transitions={
'succeeded': 'prepare_delivery_goal',
'fail_grasp': 'say_not_grasp',
'fail_recognize': 'say_object_not_find'
})
# FAIL Grasp Object
smach.StateMachine.add(
'say_not_grasp',
text_to_say("it was impossible to grasp the Object"),
transitions={
'succeeded': 'decide_next_object',
'aborted': 'aborted',
'preempted': 'preempted'
})
# FAIL Recognize Object
smach.StateMachine.add(
'say_object_not_find',
text_to_say("it was not possible to find the Object"),
transitions={
'succeeded': 'decide_next_object',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('prepare_delivery_goal',
prepare_delivery(),
transitions={
'succeeded': 'say_going_deliver_object',
'aborted': 'say_going_deliver_object',
'preempted': 'preempted'
})
# Deliver object
smach.StateMachine.add(
'say_going_deliver_object',
text_to_say("i am going to deliver the object"),
transitions={
'succeeded': 'go_to_delivery',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Go to the delivery place
smach.StateMachine.add('go_to_delivery',
nav_to_poi(),
transitions={
'succeeded': 'say_deliver_object',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Deliver object
smach.StateMachine.add(
'say_deliver_object',
text_to_say("I'm going to deliver the object"),
transitions={
'succeeded': 'deliver_object',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('deliver_object',
place_object_sm(pose_place),
transitions={
'succeeded': 'check_loop',
'aborted': 'check_loop',
'preempted': 'preempted'
})
# End of loop?
smach.StateMachine.add('check_loop',
checkLoop(),
transitions={
'succeeded': 'decide_next_object',
'aborted': 'aborted',
'preempted': 'preempted',
'end': 'succeeded'
})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.nav_to_poi_name=''
self.userdata.manip_motion_to_play = ''
# Say Start basic Functionalities
smach.StateMachine.add(
'say_start_basic_functionalities',
text_to_say("I'm ready to start Basic Functionalities"),
transitions={'succeeded': 'say_going_pick_place', 'aborted': 'play_motion_state'}) # TODO before it was say_going_pick_place
# Say Go Pick and Place
smach.StateMachine.add(
'say_going_pick_place',
text_to_say("I'm going to the Pick and Place location"),
transitions={'succeeded': 'enter_start_door', 'aborted': 'enter_start_door'})
# Cross start door and go to intermediate point
smach.StateMachine.add(
'enter_start_door',
EnterRoomSM('kitchen_table'),
transitions={'succeeded': 'pick_timer', 'aborted': 'aborted',
'preempted': 'preempted'})
# Do pick and place + TimeOut
STATES = [PickPlaceSM()]
STATE_NAMES = ["do_pick_and_place"]
smach.StateMachine.add(
"pick_timer",
ConcurrenceTime(states=STATES, state_name=STATE_NAMES, timeout=180),
transitions={'succeeded': 'say_going_avoid',
'aborted':'say_going_avoid',
'time_ends': "timeout_pick_and_place"})
#
# smach.StateMachine.add(
# 'do_pick_and_place',
# PickPlaceSM(),
# transitions={'succeeded': 'play_motion_state_2', 'aborted': 'play_motion_state_2',
# 'preempted': 'preempted'})
# Say TimeOut
smach.StateMachine.add(
'timeout_pick_and_place',
text_to_say("The time for Pick and Place is finish"),
transitions={'succeeded': 'play_motion_state', 'aborted': 'play_motion_state',
'preempted': 'preempted'})
# Home position just in case that we are in pick position
smach.StateMachine.add(
'play_motion_state',
play_motion_sm('home', skip_planning=True),
transitions={'succeeded': 'say_going_avoid',
'preempted':'say_going_avoid',
'aborted':'play_motion_state'})
# Say Go Avoid that
smach.StateMachine.add(
'say_going_avoid',
text_to_say("I'm going to the Avoid that Area"),
transitions={'succeeded': 'go_avoid_that', 'aborted': 'go_avoid_that'})
# Go to avoid that
smach.StateMachine.add(
'go_avoid_that',
nav_to_poi('avoid_that'),
transitions={'succeeded': 'wait_avoid_that', 'aborted': 'go_avoid_that',
'preempted': 'preempted'})
# Wait for Go command
smach.StateMachine.add(
'wait_avoid_that',
ListenWordSM_Concurrent(SENTENCE_GO),
transitions={'succeeded': 'confirm_move_avoid_that', 'aborted': 'wait_avoid_that',
'preempted': 'preempted'})
smach.StateMachine.add(
'confirm_move_avoid_that',
acknowledgment("yes", "Okey, I move"),
transitions={'succeeded': 'avoid_timer', 'aborted': 'say_going_avoid',
'preempted': 'preempted'})
# Do avoid that + TimeOut
STATES = [Avoid_That()]
STATE_NAMES = ["do_avoid_that"]
smach.StateMachine.add(
"avoid_timer",
ConcurrenceTime(states=STATES, state_name=STATE_NAMES, timeout=180),
transitions={'succeeded': 'say_going_what_say',
'aborted':'say_going_what_say',
'time_ends': "timeout_avoid_that"})
# Say TimeOut
smach.StateMachine.add(
'timeout_avoid_that',
text_to_say("The time for Avoid That is finish"),
transitions={'succeeded': 'say_going_what_say', 'aborted': 'say_going_what_say',
'preempted': 'preempted'})
# Do avoid that
# smach.StateMachine.add(
# 'do_avoid_that',
# Avoid_That(),
# transitions={'succeeded': 'say_going_what_say', 'aborted': 'say_going_what_say',
# 'preempted': 'preempted'})
# Say Go What did you say
smach.StateMachine.add(
'say_going_what_say',
text_to_say("I'm ready to start the What did you say test"),
transitions={'succeeded': 'wait_what_did_you_say', 'aborted': 'wait_what_did_you_say'})
# Wait for Go command
smach.StateMachine.add(
'wait_what_did_you_say',
ListenWordSM_Concurrent(SENTENCE_GO),
transitions={'succeeded': 'confirm_move_what_say', 'aborted': 'wait_what_did_you_say',
'preempted': 'preempted'})
smach.StateMachine.add(
'confirm_move_what_say',
acknowledgment("yes", "Okey, I go"),
transitions={'succeeded': 'what_say_timer', 'aborted': 'what_say_timer',
'preempted': 'preempted'})
# Do what did you say + TimeOut
STATES = [WhatSaySM()]
STATE_NAMES = ["do_what_did_you_say"]
smach.StateMachine.add(
"what_say_timer",
ConcurrenceTime(states=STATES, state_name=STATE_NAMES, timeout=180),
transitions={'succeeded': 'go_to_door',
'aborted':'go_to_door',
'time_ends': "timeout_what_say"})
# Say TimeOut
smach.StateMachine.add(
'timeout_what_say',
text_to_say("The time for What did you say is finish"),
transitions={'succeeded': 'go_to_door', 'aborted': 'go_to_door',
'preempted': 'preempted'})
# Do what did you say
# smach.StateMachine.add(
# 'do_what_did_you_say',
# WhatSaySM(),
# transitions={'succeeded': 'say_finish_basic_functionalities', 'aborted': 'say_finish_basic_functionalities',
# 'preempted': 'preempted'})
# Go to the exit door
smach.StateMachine.add(
'go_to_door',
nav_to_poi('door_exit'),
transitions={'succeeded': 'go_to_exit', 'aborted': 'go_to_exit',
'preempted': 'preempted'})
# Go to the exit door
smach.StateMachine.add(
'go_to_exit',
nav_to_poi('door_B'),
transitions={'succeeded': 'say_finish_basic_functionalities', 'aborted': 'say_finish_basic_functionalities',
'preempted': 'preempted'})
# Say Finish basic Functionalities
smach.StateMachine.add(
'say_finish_basic_functionalities',
text_to_say("I finished Basic Functionalities"),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
def __init__(self, poi_name=None, time_out=10):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['name', 'nav_to_poi_name', 'face', 'face_frame'],
output_keys=['face', 'standard_error', 'face_frame'])
self.poi_name = poi_name
self.time_out = time_out
with self:
self.userdata.name = ''
self.userdata.num_iterations = 0
self.userdata.face = None
self.userdata.wait_time = 5
self.userdata.nav_to_poi_name = None
self.userdata.face_frame = None
# We define the different points
smach.StateMachine.add('prepare_poi',
prepare_poi(self.poi_name),
transitions={
'poi': 'go_poi',
'no_poi': 'Concurrence'
})
smach.StateMachine.add('Say_Searching',
text_to_say(
'Right Now I am looking for you.',
wait=True),
transitions={
'succeeded': 'go_poi',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add(
'go_poi',
nav_to_poi(poi_name),
transitions={
'succeeded': 'Concurrence',
'aborted': 'Concurrence', #TODO
'preempted': 'preempted'
})
# Concurrence
sm_conc = smach.Concurrence(
outcomes=['succeeded', 'aborted', 'preempted', 'endTime'],
default_outcome='succeeded',
input_keys=['name', 'nav_to_poi_name', 'face', 'wait_time'],
output_keys=['face', 'standard_error', 'face_frame'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# Go around the room
# smach.Concurrence.add('walk_to_poi', nav_to_poi())
#smach.Concurrence.add('turn', turn_infinit('left'))
# Move head
smach.Concurrence.add('TimeOut', TimeOut(self.time_out))
# Search for face
smach.Concurrence.add('find_faces',
recognize_face_concurrent())
smach.Concurrence.add('say_search_faces',
say_searching_faces())
smach.StateMachine.add('Concurrence',
sm_conc,
transitions={
'succeeded': 'DEFAULT_POSITION',
'aborted': 'Say_Finish',
'endTime': 'Say_Finish',
'preempted': 'Say_Finish'
})
smach.StateMachine.add('Say_Finish',
text_to_say(
'I have finish with no exit the search',
wait=False),
transitions={
'succeeded': 'DEFAULT_POSITION_NO',
'aborted': 'DEFAULT_POSITION_NO',
'preempted': 'preempted'
})
smach.StateMachine.add('DEFAULT_POSITION_NO',
move_head_form("center", "up"),
transitions={
'succeeded': 'aborted',
'aborted': 'aborted'
})
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center", "up"),
transitions={
'succeeded': 'succeeded',
'aborted': 'succeeded'
})
def __init__(self):
sm = smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
smach.StateMachine.add(
'Say_Init',
text_to_say("Initializing and Preparing Emergency Test. Let's go!"),
transitions={'succeeded':'Arms_Home'})
# Put the arms at the 'Home' position, in order improve the security
# We prepare the information to go to the init door
self.userdata.manip_motion_to_play = 'home'
self.userdata.manip_time_to_play = 4.0
smach.StateMachine.add(
'Arms_Home',
play_motion_sm(),
transitions={'succeeded':'Prepare_TTS_1', 'aborted':'Prepare_TTS_1', 'preempted':'Prepare_TTS_1'})
#Prepare the data for the speech: language and time to wait
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_lang = 'en_US'
smach.StateMachine.add(
'Prepare_TTS_1',
prepare_tts("Now I am going to enter the Room to help people in need."),
transitions={'succeeded':'Say_Ready'})
smach.StateMachine.add(
'Say_Ready',
text_to_say(),
transitions={'succeeded':'Enter_Room_Arena', 'aborted':'Enter_Room_Arena', 'preempted':'Enter_Room_Arena'})
# Pass the entry room
# TODO: Define the poi for the output of the room
# Pre: The robot should be in front of the Arena door (maybe we should change this, depending on the conditions)
#smach.StateMachine.add(
# 'Prepare_Door_Out_Arena',
# prepare_poi_emergency('entry_door_exit'),
# transitions={'succeeded':'Enter_Room_Arena', 'aborted':'Enter_Room_Arena', 'preempted':'Enter_Room_Arena'})
smach.StateMachine.add(
'Enter_Room_Arena',
EnterRoomSM('entry_door_exit'),
transitions={'succeeded':'Say_Enter_Successful', 'aborted':'Prepare_TTS_1', 'preempted':'Prepare_Poi_Emergency_1'})
smach.StateMachine.add(
'Say_Enter_Successful',
text_to_say('Successfully entering the room. Now I am going to the Emergency Room.'),
transitions={'succeeded':'Prepare_Poi_Emergency_1', 'aborted':'Prepare_Poi_Emergency_1', 'preempted':'Prepare_Poi_Emergency_1'})
# Go to the emergency location
# TODO: Define the name of the room to enter (defined by the OC)
# If Aborted (not supposed to), retry?
smach.StateMachine.add(
'Prepare_Poi_Emergency_1',
prepare_poi_emergency('emergency_room'),
transitions={'succeeded':'Go_to_emergency_room'})
smach.StateMachine.add(
'Go_to_emergency_room',
nav_to_poi(),
transitions={'succeeded':'Search_Person', 'aborted':'Go_to_emergency_room', 'preempted':'Go_to_emergency_room'})
# Userdata output keys:
# - person_location: PoseStamped/-->Pose<-- (?)
# Another state will be needed (maybe) to remap
# No need of face_recognition
# What if person not found? Re-search?
smach.StateMachine.add(
'Search_Person',
Search_People_Emergency(),
transitions={'succeeded':'Save_Person', 'aborted':'Search_Person', 'preempted':'Search_Person'})
# Userdata input:
# person_location: PoseStamped (?)
# It is a SuperStateMachine (contains submachines) with these functionalities (draft):
# 1. Go to Person location
# 2. Ask Status
# 3. Register position
# 4. Save info
# What to do if fail?
smach.StateMachine.add(
'Save_Person',
Save_People_Emergency(),
transitions={'succeeded':'Get_Person_Desired_Object', 'aborted':'Save_Person', 'preempted':'Save_Person'})
# The functionalities of this SuperSM are:
# 1. Ask the person what to fetch
# 2. Go and grab the object --> Similar with Pick-and-Place
# 2.1. Go to room
# 2.2. Find Object
# 2.3. Go to Object
# 2.4. Grab Object
# 2.5. Go to person
# 2.6. Give object --> Ungrab
#--> Database of objects and their location
# --> Manip/Grab
#
smach.StateMachine.add(
'Get_Person_Desired_Object',
Get_Person_Desired_Object(),
transitions={'succeeded':'Prepare_home', 'aborted':'Get_Person_Desired_Object', 'preempted':'Prepare_home'})
#TODO: Define Entry room POI: userdata.nav_poi (?)
#Retrying to go to entry_door until is succeeded
smach.StateMachine.add(
'Prepare_home',
set_home_position(),
transitions={'succeeded':'SetHome', 'aborted':'SetHome', 'preempted':'SetHome'})
smach.StateMachine.add(
'SetHome',
play_motion_sm(),
transitions={'succeeded':'Go_to_Entry_Door', 'aborted':'SetHome', 'preempted':'SetHome'})
smach.StateMachine.add(
'Go_to_Entry_Door',
nav_to_poi('entry_door_init'),
transitions={'succeeded':'Wait_for_Ambulance_Person', 'aborted':'Go_to_Entry_Door', 'preempted':'Go_to_Entry_Door'})
#What is Wait for Ambulance or People Mean? Person detection?
smach.StateMachine.add(
'Wait_for_Ambulance_Person',
DummyStateMachine(),
transitions={'succeeded':'Prepare_TTS_2', 'aborted':'Go_to_Entry_Door', 'preempted':'Go_to_Entry_Door'})
smach.StateMachine.add(
'Prepare_TTS_2',
prepare_tts("Please Follow Me, I will guide you to the emergency"),
transitions={'succeeded':'Say_Ambulance', 'aborted':'Say_Ambulance', 'preempted':'Say_Ambulance'})
smach.StateMachine.add(
'Say_Ambulance',
text_to_say(),
transitions={'succeeded':'Go_to_emergency_room_2', 'aborted':'Go_to_emergency_room_2', 'preempted':'Go_to_emergency_room_2'})
#TODO: Define the name of the room to enter (defined by the OC)
#If Aborted (not supposed to), retry?
smach.StateMachine.add(
'Go_to_emergency_room_2',
nav_to_poi('emergency_room'),
transitions={'succeeded':'Wait_state_emergency', 'aborted':'Go_to_emergency_room_2', 'preempted':'Go_to_emergency_room_2'})
smach.StateMachine.add(
'Wait_state_emergency',
DummyStateMachine(),
transitions={'succeeded':'Say_Finish', 'aborted':'Say_Finish', 'preempted':'Say_Finish'})
smach.StateMachine.add(
'Say_Finish',
text_to_say('Here is the Emergency. I helped to save a life.'),
transitions={'succeeded':'succeeded', 'aborted':'aborted', 'preempted':'preempted'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.nav_to_poi_name=''
self.userdata.tts_lang = ''
self.userdata.tts_wait_before_speak = ''
self.userdata.tts_text = ''
# Say start Pick and Place
smach.StateMachine.add(
'say_start_pick_place',
text_to_say("I'm going to start Pick and Place test"),
transitions={'succeeded': 'say_start_obj_recognition', 'aborted': 'say_start_obj_recognition'})
# Say Going to location
# smach.StateMachine.add(
# 'say_go_location',
# text_to_say("I'm going to the location where I have to recognize some objects",wait=False),
# transitions={'succeeded': 'prepare_location', 'aborted': 'prepare_location'})
#
# # Prepare the poi for nav_to_poi
# smach.StateMachine.add(
# 'prepare_location',
# prepare_location(),
# transitions={'succeeded': 'go_location', 'aborted': 'prepare_location',
# 'preempted': 'preempted'})
#
# # Go to the location
# smach.StateMachine.add(
# 'go_location',
# nav_to_poi(),
# transitions={'succeeded': 'say_start_obj_recognition', 'aborted': 'say_go_location',
# 'preempted': 'preempted'})
# Say start object recognition
smach.StateMachine.add(
'say_start_obj_recognition',
text_to_say("I'm going to start the Object recognition"),
transitions={'succeeded': 'object_recognition', 'aborted': 'object_recognition'})
# Do object_recognition
smach.StateMachine.add(
'object_recognition',
dummy_recognize(),
transitions={'succeeded': 'say_grasp_object', 'aborted': 'say_release_obj',
'preempted': 'preempted'})
# Say grasp object
smach.StateMachine.add(
'say_grasp_object',
text_to_say("I'm going to grasp the object"),
transitions={'succeeded': 'grasp_object', 'aborted': 'grasp_object'})
# Grasp the object
smach.StateMachine.add(
'grasp_object',
pick_object_sm(),
transitions={'succeeded': 'say_go_second_location', 'aborted': 'play_motion_grasp',
'preempted': 'preempted'})
# Home position
smach.StateMachine.add(
'play_motion_grasp',
play_motion_sm('home'),
transitions={'succeeded': 'say_grasp_object', 'preempted':'say_grasp_object',
'aborted':'play_motion_grasp'})
# Say go to second location
smach.StateMachine.add(
'say_go_second_location',
text_to_say("I'm going to the location where I should release the object"),
transitions={'succeeded': 'go_second_location', 'aborted': 'go_second_location'})
# Go the location - We need to go to the place to object category, so we assume that the
# object recognition will init the poi to the object must to go
smach.StateMachine.add(
'go_second_location',
nav_to_poi(),
transitions={'succeeded': 'say_release_obj', 'aborted': 'say_go_second_location',
'preempted': 'preempted'})
# Say release object
smach.StateMachine.add(
'say_release_obj',
text_to_say("I'm going to release the object"),
transitions={'succeeded': 'pregrasp_state', 'aborted': 'pregrasp_state'})
# Pre-grasp position
smach.StateMachine.add(
'pregrasp_state',
play_motion_sm('pre_grasp', skip_planning=True),
transitions={'succeeded': 'play_motion_state', 'preempted':'play_motion_state',
'aborted':'pregrasp_state'})
# Release the object
smach.StateMachine.add(
'release_object',
place_object_sm(),
transitions={'succeeded': 'play_motion_state', 'aborted': 'say_release_obj',
'preempted': 'preempted'})
# Home position
smach.StateMachine.add(
'play_motion_state',
play_motion_sm('home'),
transitions={'succeeded': 'say_end_pick_place',
'preempted':'say_end_pick_place',
'aborted':'say_end_pick_place'})
# Say end Pick and Place
smach.StateMachine.add(
'say_end_pick_place',
text_to_say("I finished the Pick and Place test"),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})
def __init__(self, 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'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.nav_to_poi_name=''
self.userdata.tts_time_before_speaking = 0
self.userdata.tts_text = ""
self.userdata.tts_lang = ""
# Indicate that we are ready
smach.StateMachine.add(
'say_ready_inspection',
text_to_say("I'm ready for Robot Inspection test"),
transitions= {'succeeded':'enter_start_door', 'aborted':'enter_start_door', 'preempted':'preempted'})
# Cross start door and go to intermediate point
smach.StateMachine.add(
'enter_start_door',
EnterRoomSM('intermediate'),
transitions={'succeeded': 'robot_presentation', 'aborted': 'aborted',
'preempted': 'preempted'})
# Robot presentation: Little talk + wave gesture
STATES = [text_to_say("Hi everybody! My name is REEM."), play_motion_sm("wave")]
STATE_NAMES = ["say_presentation", "salute_wave"]
outcome_map = {'succeeded': {"say_presentation": 'succeeded', "salute_wave": 'succeeded'}}
smach.StateMachine.add(
"robot_presentation",
ConcurrenceRobocup(states=STATES, state_names=STATE_NAMES, outcome_map=outcome_map),
transitions={'succeeded': 'home_position', 'aborted': "robot_presentation"})
# Home position
smach.StateMachine.add(
'home_position',
play_motion_sm(motion='home', skip_planning=True),
transitions={'succeeded': 'wait_inspection', 'aborted': 'home_position', 'preempted': 'succeeded'})
# Sleep for inspection
smach.StateMachine.add(
'wait_inspection',
Sleeper(5),
transitions={'succeeded': 'say_end_time_inspection', 'aborted': 'say_end_time_inspection', 'preempted': 'succeeded'})
# Indicate that we are ready to go out
smach.StateMachine.add(
'say_end_time_inspection',
text_to_say("I hope you are happy with the inspection. I'll leave the room."),
transitions= {'succeeded':'go_exit', 'aborted':'go_exit', 'preempted':'preempted'})
# Go to the exit
smach.StateMachine.add(
'go_exit',
GoToExit('exit_door'),
transitions= {'succeeded':'set_robot_position', 'aborted':'set_robot_position', 'preempted':'preempted'})
# Indicate that we are ready to go out
# smach.StateMachine.add(
# 'say_save_pos',
# text_to_say("Saving position"),
# transitions= {'succeeded':'set_robot_position', 'aborted':'set_robot_position', 'preempted':'preempted'})
#
# Set position
smach.StateMachine.add(
'set_robot_position',
set_robot_position(),
transitions={'succeeded': 'cross_door_out', 'aborted': 'set_robot_position',
'preempted': 'preempted'})
# Go to the exit door and cross exit door
# If the door is open change EnterRoom for nav_to_poi
smach.StateMachine.add(
'cross_door_out',
nav_to_poi('exit_door'),
transitions={'succeeded': 'say_exit', 'aborted': 'cross_door_out',
'preempted': 'preempted'})
# Indicate that we are ready to go
smach.StateMachine.add(
'say_exit',
text_to_say("I arrived successfully to the exit location, Robot Inspection Test complete."),
transitions= {'succeeded':'succeeded', 'aborted':'aborted', 'preempted':'preempted'})
