def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error','in_learn_person'])
with self:
self.userdata.tts_wait_before_speaking=0
self.userdata.tts_text=None
self.userdata.tts_lang=None
self.userdata.standard_error='OK'
self.userdata.in_learn_person=1
smach.StateMachine.add('INTRO',
text_to_say(START_FRASE),
transitions={'succeeded': 'Listen','aborted':'aborted'})
smach.StateMachine.add('Listen',
ListenWordSM("follow me"),
transitions={'succeeded': 'START_FOLLOWING_COME_CLOSER',
'aborted': 'Listen'})
smach.StateMachine.add('START_FOLLOWING_COME_CLOSER',
text_to_say(START_FOLLOW_FRASE),
transitions={'succeeded': 'SM_LEARN_PERSON','aborted':'aborted'})
# it learns the person that we have to follow
smach.StateMachine.add('SM_LEARN_PERSON',
LearnPerson(),
transitions={'succeeded': 'SM_STOP_LEARNING',
'aborted': 'aborted'})
smach.StateMachine.add('SM_STOP_LEARNING',
text_to_say(LEARNED_PERSON_FRASE),
transitions={'succeeded': 'succeeded','aborted':'aborted'})
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,
['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', 'in_learn_person'])
with self:
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_text = None
self.userdata.tts_lang = None
self.userdata.standard_error = 'OK'
self.userdata.in_learn_person = 1
smach.StateMachine.add('INTRO',
text_to_say(START_FRASE),
transitions={
'succeeded': 'Learn',
'aborted': 'aborted'
})
# it learns the person that we have to follow
smach.StateMachine.add('Learn',
LearnPerson(),
transitions={
'succeeded': 'SM_STOP_LEARNING',
'aborted': 'aborted'
})
smach.StateMachine.add('SM_STOP_LEARNING',
text_to_say(LETS_GO),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
output_keys=[])
with self:
self.userdata.pressed = False
self.userdata.press = False
self.userdata.tts_time_before_speaking = 0
self.userdata.tts_text = ""
self.userdata.tts_lang = ""
# Check if the button is pressed
smach.StateMachine.add(
'check_button',
emergency_button(),
transitions= {'succeeded':'check_button_pressed', 'aborted':'check_button', 'preempted':'preempted'})
smach.StateMachine.add(
'check_button_pressed',
check_button_pressed(),
transitions= {'pressed':'say_pressed_button', 'unpressed':'say_not_pressed_button',
'succeeded':'check_button', 'preempted':'preempted'})
# Say button pressed
smach.StateMachine.add(
'say_pressed_button',
text_to_say("Oh you pressed my emergency button, feel free to check me around"),
transitions= {'succeeded':'check_button', 'aborted':'check_button', 'preempted':'preempted'})
# Say button is not pressed
smach.StateMachine.add(
'say_not_pressed_button',
text_to_say("Thank you for unpressing my emergency button. I'll recover my status shortly."),
transitions= {'succeeded':'succeeded', 'aborted':'succeeded', 'preempted':'preempted'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
output_keys=['person_location', 'person_location_coord'])
with self:
self.userdata.emergency_location = []
# Some dummy TTS stuff
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
smach.StateMachine.add(
'Prepare_Say_Searching',
prepare_tts('Where are you? Give me signals, please.'),
transitions={'succeeded':'Say_Search', 'aborted':'Say_Search', 'preempted':'Say_Search'})
smach.StateMachine.add(
'Say_Search',
text_to_say(),
transitions={'succeeded':'Gesture_Recognition', 'aborted':'Gesture_Recognition', 'preempted':'Gesture_Recognition'})
# Search for a Wave Gesture
# Output_keys: gesture_detected: type Gesture
self.userdata.nav_to_coord = [0, 0, 0]
#Look Down
#Move head right/left
smach.StateMachine.add(
'Gesture_Recognition',
WaveDetection(),
transitions={'succeeded':'Say_Search','aborted':'Gesture_Recognition', 'preempted':'Gesture_Recognition'})
smach.StateMachine.add(
'Say_Search',
text_to_say('Oh! I have found you.'),
transitions={'succeeded':'Prepare_Go_To_Wave', 'aborted':'Prepare_Go_To_Wave', 'preempted':'Say_Search'})
smach.StateMachine.add(
'Prepare_Go_To_Wave',
prepare_go_to_wave(),
transitions={'succeeded':'Say_Go_to_Wave', 'aborted':'Say_Go_to_Wave', 'preempted':'Say_Go_to_Wave'})
smach.StateMachine.add(
'Say_Go_to_Wave',
text_to_say("I'm coming!"),
transitions={'succeeded':'Go_to_Wave', 'aborted':'Gesture_Recognition', 'preempted':'Gesture_Recognition'})
smach.StateMachine.add(
'Go_to_Wave',
nav_to_coord('/base_link'),
transitions={'succeeded':'Register_Position', 'aborted':'Go_to_Wave', 'preempted':'Go_to_Wave'})
smach.StateMachine.add(
'Register_Position',
get_current_robot_pose(),
transitions={'succeeded':'TreatPoseForCoord', 'aborted':'Register_Position', 'preempted':'Register_Position'},
remapping={'current_robot_pose':'person_location'})
smach.StateMachine.add(
'TreatPoseForCoord',
PoseToArray(),
transitions={'succeeded':'succeeded', 'aborted':'Register_Position', 'preempted':'Register_Position'})
def __init__(self,learn_face=False):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error','in_learn_person'])
self.learn_face = learn_face
self.follow_pub = rospy.Publisher('/follow_me/id', Int32, latch=True)
with self:
self.userdata.tts_wait_before_speaking=0
self.userdata.tts_text=None
self.userdata.tts_lang=None
self.userdata.standard_error='OK'
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={'succeeded': 'READ_TRACKER_TOPIC',
'aborted': 'SAY_LIFTIME'})
smach.StateMachine.add('SAY_LIFTIME',
text_to_say(SAY_GO_MIDLE),
transitions={'succeeded': 'CONTROL_TIME',
'aborted': 'SAY_LIFTIME'})
smach.StateMachine.add('CONTROL_TIME',
control_time(),
transitions={'succeeded': 'READ_TRACKER_TOPIC',
'lif_time': 'SAY_LIFTIME'})
#TODO:: aborted->CONTROL_TIME
smach.StateMachine.add('READ_TRACKER_TOPIC',
topic_reader(topic_name='/pipol_tracker_node/peopleSet',
topic_type=personArray,topic_time_out=60),
transitions={'succeeded':'SELECT_ID',
'aborted':'READ_TRACKER_TOPIC',
'preempted':'preempted'},
remapping={'topic_output_msg': 'tracking_msg'})
# it learns the person that we have to follow
smach.StateMachine.add('SELECT_ID',
select_ID(self.learn_face, self.follow_pub),
transitions={'succeeded': 'STOP_LEARNING',
'aborted': 'CONTROL_TIME','learn_face':'FACE_SAY'})
smach.StateMachine.add('FACE_SAY',
text_to_say(SAY_LEARN),
transitions={'succeeded': 'LEARN_FACE','aborted':'LEARN_FACE'})
# it learns the person that we have to follow
smach.StateMachine.add('LEARN_FACE',
new_database_and_learn(learning_time=2, minConfidence=80,
name_face="following", name_database="follow"),
transitions={'succeeded': 'STOP_LEARNING',
'aborted': 'CONTROL_TIME'})
smach.StateMachine.add('STOP_LEARNING',
text_to_say(SAY_FINISH),
transitions={'succeeded': 'succeeded','aborted':'STOP_LEARNING'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.loop_iterations = 1
self.userdata.try_iterations = 1
self.userdata.gesture_name = ''
self.userdata.object_name = []
self.userdata.manip_time_to_play = 4
self.userdata.did_pick = True
self.userdata.grammar_name = GRAMMAR_NAME
smach.StateMachine.add(
'play_motion_state',
play_motion_sm('home', skip_planning=True),
transitions={'succeeded': 'init_cocktail',
'preempted':'init_cocktail',
'aborted':'play_motion_state'})
smach.StateMachine.add(
'init_cocktail',
text_to_say("Ready for cocktail party"),
transitions={'succeeded': 'Ask_order', 'aborted': 'Ask_order'})
# We wait for open door and go inside
smach.StateMachine.add(
'wait_for_door',
EnterRoomSM("party_room"),
transitions={'succeeded': 'Ask_order', 'aborted': 'aborted', 'preempted': 'preempted'})
# Ask Order -> Wave + Learn Person + Order
smach.StateMachine.add(
'Ask_order',
AskAllOrders(),
transitions={'succeeded':'execute_order', 'aborted':'aborted'})
# # Go to the storage_room
# smach.StateMachine.add(
# 'go_to_storage',
# nav_to_poi("storage_room"),
# transitions={'succeeded': 'execute_order', 'aborted': 'go_to_storage',
# 'preempted': 'preempted'})
# Execute the order
smach.StateMachine.add(
'execute_order',
ExecuteAllOrders(),
transitions={'succeeded': 'say_leaving_arena', 'aborted': 'aborted',
'preempted': 'preempted'})
# Say leaving the arena
smach.StateMachine.add(
'say_leaving_arena',
text_to_say("I finished the cocktail party, I'm leaving the arena", wait=False),
transitions={'succeeded': 'succeeded', 'aborted': 'succeeded',
'preempted': 'preempted'})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', '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'],
output_keys=['standard_error', 'in_learn_person'])
with self:
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_text = None
self.userdata.tts_lang = None
self.userdata.head_left_right = None
self.userdata.head_up_down = None
self.userdata.standard_error = 'OK'
self.userdata.in_learn_person = 1
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center", "up"),
transitions={
'succeeded': 'INTRO',
'aborted': 'INTRO'
})
smach.StateMachine.add('INTRO',
text_to_say(START_FRASE),
transitions={
'succeeded':
'START_FOLLOWING_COME_CLOSER',
'aborted': 'START_FOLLOWING_COME_CLOSER'
})
smach.StateMachine.add('Listen',
ListenWordSM_Concurrent("follow me"),
transitions={
'succeeded':
'START_FOLLOWING_COME_CLOSER',
'aborted': 'Listen'
})
smach.StateMachine.add('START_FOLLOWING_COME_CLOSER',
text_to_say(START_FOLLOW_FRASE),
transitions={
'succeeded': 'SM_LEARN_PERSON',
'aborted': 'aborted'
})
# it learns the person that we have to follow
smach.StateMachine.add('SM_LEARN_PERSON',
LearnPerson(learn_face=True),
transitions={
'succeeded': 'SM_STOP_LEARNING',
'aborted': 'aborted'
})
smach.StateMachine.add('SM_STOP_LEARNING',
text_to_say(LEARNED_PERSON_FRASE),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
def __init__(self):
smach.StateMachine.__init__(
self, ['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error', 'in_learn_person'])
with self:
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_text = None
self.userdata.tts_lang = None
self.userdata.standard_error = 'OK'
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={
'succeeded': 'SAY_LIFTIME',
'aborted': 'SAY_LIFTIME'
})
smach.StateMachine.add('SAY_LIFTIME',
text_to_say(SAY_GO_MIDLE),
transitions={
'succeeded': 'CONTROL_TIME',
'aborted': 'SAY_LIFTIME'
})
smach.StateMachine.add('CONTROL_TIME',
control_time(),
transitions={
'succeeded': 'READ_TRACKER_TOPIC',
'lif_time': 'INIT_VAR'
})
smach.StateMachine.add(
'READ_TRACKER_TOPIC',
topic_reader(topic_name='/people_tracker_node/peopleSet',
topic_type=personArray,
topic_time_out=60),
transitions={
'succeeded': 'SELECT_ID',
'aborted': 'READ_TRACKER_TOPIC',
'preempted': 'preempted'
},
remapping={'topic_output_msg': 'tracking_msg'})
# it learns the person that we have to follow
smach.StateMachine.add('SELECT_ID',
select_ID(),
transitions={
'succeeded': 'STOP_LEARNING',
'aborted': 'CONTROL_TIME'
})
smach.StateMachine.add('STOP_LEARNING',
text_to_say("OK i have finsished"),
transitions={
'succeeded': 'succeeded',
'aborted': 'STOP_LEARNING'
})
def __init__(self,time_enroll=10):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['name_face','delete_database'],
output_keys=['name_face'])
with self:
self.userdata.name_face = ''
self.userdata.asr_userSaid = ''
self.userdata.grammar_name = ''
# Ask for name
smach.StateMachine.add(
'listen_name',
AskQuestionSM("Hi, what's your name?", GRAMMAR_NAME),
transitions={'succeeded': 'prepare_name', 'aborted': 'ask_name_again',
'preempted': 'preempted'})
# We prepare the name for face_detection
smach.StateMachine.add(
"prepare_name",
prepare_name(),
transitions={'succeeded': 'say_start', 'aborted': 'ask_name_again',
'preempted': 'preempted'})
# Ask for name again
smach.StateMachine.add(
'ask_name_again',
AskQuestionSM("Sorry, I don't understand you. Can you repeat your name, please?", GRAMMAR_NAME),
transitions={'succeeded': 'prepare_name', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'say_start',
text_to_say("OK, now i am going to enroll your face, don't move"),
transitions={'succeeded': 'learn_face', 'aborted': 'aborted',
'preempted': 'preempted'})
# Start learning
smach.StateMachine.add(
'learn_face',
new_database_and_learn(time_enroll),
transitions={'succeeded': 'prepare_say_name', 'aborted': 'aborted',
'preempted': 'preempted'})
# Say learn name
smach.StateMachine.add(
'prepare_say_name',
prepare_say_name(),
transitions={'succeeded': 'say_name', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'say_name',
text_to_say(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
def main():
rospy.init_node('yes_no_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.grammar_name = None
sm.userdata.tts_text = None
sm.userdata.tts_wait_before_speaking = None
sm.userdata.tts_lang = None
# Load grammar yes/no
smach.StateMachine.add('ActivateASR_yesno',
ActivateASR("robocup/yes_no"),
transitions={
'succeeded': 'DID_YOU_SAY',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('DID_YOU_SAY',
text_to_say("ready"),
transitions={
'succeeded': 'YesNoTest',
'aborted': 'aborted'
})
smach.StateMachine.add('YesNoTest',
SayYesOrNoSM(),
transitions={
'succeeded': 'SAY_SUCC',
'aborted': 'SAY_ABORTED'
})
smach.StateMachine.add('SAY_SUCC',
text_to_say("SUCCEEDED"),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
smach.StateMachine.add('SAY_ABORTED',
text_to_say("ABORTED"),
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('yes_no_introspection', sm,
'/YES_NO_TEST')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def __init__(self, Type, objectName, GRAMMAR_NAME='robocup/locations', command_key='finn', command_value='xxx'):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
self.userdata.dataType = Type
self.userdata.object_name = objectName
self.userdata.location_name = ''
self.userdata.grammar_name = GRAMMAR_NAME
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_text = ''
self.userdata.tts_lang = ''
with self:
smach.StateMachine.add('ENABLE_GRAMMAR',
ActivateASR(GRAMMAR_NAME),
transitions={'succeeded': 'ASK_LOCATION'})
smach.StateMachine.add('ASK_LOCATION',
text_to_say("I don't know where the " + self.userdata.object_name + ' is. Do you know where could I find it?'),
transitions={'succeeded': 'HEAR_COMMAND', 'aborted': 'aborted'})
smach.StateMachine.add('HEAR_COMMAND',
ReadASR(),
transitions={'aborted': 'HEAR_COMMAND', 'succeeded': 'BRING_LOCATION', 'preempted': 'preempted'},
remapping={'asr_userSaid': 'userSaidData', 'asr_userSaid_tags':'userSaidTags'})
smach.StateMachine.add('BRING_LOCATION',
BringOrderLoc(),
transitions={'aborted': 'HEAR_COMMAND', 'succeeded': 'PREPARATION_CONFIRM_OBJECT', 'preempted': 'preempted'},
remapping={'location_name': 'location_name'})
smach.StateMachine.add('PREPARATION_CONFIRM_OBJECT',
prepare_confirm_info(),
transitions={'succeeded': 'CONFIRM_OBJECT'},
remapping={'tosay':'tts_text'})
smach.StateMachine.add('CONFIRM_OBJECT',
text_to_say(),
transitions={'succeeded': 'DISABLE_GRAMMAR', 'aborted': 'DISABLE_GRAMMAR'})
smach.StateMachine.add('RECOGNIZE_COMMAND',
RecognizeCommand(command_key, command_value),
transitions={'notvalid_command': 'NOT_VALID_COMMAND',
'valid_command': 'VALID_COMMAND', 'preempted': 'preempted', 'aborted': 'aborted'},
remapping={'speechData': 'userSaidData'})
smach.StateMachine.add('VALID_COMMAND',
text_to_say("Ok, understood."),
transitions={'succeeded': 'DISABLE_GRAMMAR'})
smach.StateMachine.add('NOT_VALID_COMMAND',
text_to_say("I couldn't understand what you said. Can you repeat?"),
transitions={'succeeded': 'HEAR_COMMAND'})
smach.StateMachine.add('DISABLE_GRAMMAR',
DeactivateASR(),
transitions={'succeeded': 'succeeded'})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted', 'fail_grasp', 'fail_recognize'],
input_keys=['object_name'],
output_keys=['object_position','object_detected_name'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.nav_to_poi_name=''
self.userdata.tts_lang = ''
self.userdata.tts_wait_before_speak = ''
self.userdata.tts_text = ''
# Say start object recognition
smach.StateMachine.add(
'say_start_obj_recognition',
text_to_say("I'm going to start the Object recognition process.", wait=False),
transitions={'succeeded': 'object_recognition', 'aborted': 'object_recognition'})
# Do object_recognition
smach.StateMachine.add(
'object_recognition',
recognize_object(),
transitions={'succeeded': 'process_object_recognition', 'aborted': 'fail_recognize',
'preempted': 'preempted'})
# Process the objects recognized
smach.StateMachine.add(
'process_object_recognition',
process_pick_location(),
transitions={'succeeded': 'say_grasp_object', 'aborted': 'fail_recognize',
'preempted': 'preempted'})
# Say grasp object
smach.StateMachine.add(
'say_grasp_object',
text_to_say("I'm going to grasp the object", wait=False),
transitions={'succeeded': 'grasp_object', 'aborted': 'aborted'})
# Grasp the object
smach.StateMachine.add(
'grasp_object',
pick_object_sm(),
transitions={'succeeded': 'succeeded', 'aborted': 'home_position',
'preempted': 'preempted'})
# Home position
smach.StateMachine.add(
'home_position',
play_motion_sm('home',skip_planning=True),
transitions={'succeeded': 'fail_grasp', 'aborted': 'home_position',
'preempted': 'preempted'})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
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 main():
rospy.init_node('Conc_time_node')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.sleep_time = 10
STATES = [Sleeper(30)]
STATE_NAMES = ["sleep"]
INPUTS = ["sleep_time"]
smach.StateMachine.add('Conc_time',
ConcurrenceTime(state_name=STATE_NAMES, states=STATES, inputs=INPUTS, timeout=5),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted', 'time_ends':'say_time_out'})
# Say TimeOut
smach.StateMachine.add(
'say_time_out',
text_to_say("The time is finish"),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
sm.execute()
rospy.spin()
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error','in_learn_person'])
with self:
self.userdata.tts_wait_before_speaking=0
self.userdata.tts_text=None
self.userdata.tts_lang=None
self.userdata.standard_error='OK'
self.userdata.in_learn_person=1
self.userdata.type_movment = 'home'
smach.StateMachine.add('INTRO',
acknowledgment(type_movement='home', tts_text=START_FRASE),
transitions={'succeeded': 'DEFAULT_POSITION','aborted':'DEFAULT_POSITION'})
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center","up"),
transitions={'succeeded': 'Learn','aborted':'Learn'})
# it learns the person that we have to follow
smach.StateMachine.add('Learn',
LearnPerson(learn_face=True),
transitions={'succeeded': 'SM_STOP_LEARNING',
'aborted': 'aborted'})
smach.StateMachine.add('SM_STOP_LEARNING',
text_to_say(LETS_GO),
transitions={'succeeded': 'succeeded','aborted':'aborted'})
def main():
rospy.init_node('Conc_time_node')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.sleep_time = 10
STATES = [Sleeper(30)]
STATE_NAMES = ["sleep"]
INPUTS = ["sleep_time"]
smach.StateMachine.add('Conc_time',
ConcurrenceTime(state_name=STATE_NAMES,
states=STATES,
inputs=INPUTS,
timeout=5),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'time_ends': 'say_time_out'
})
# Say TimeOut
smach.StateMachine.add('say_time_out',
text_to_say("The time is finish"),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
sm.execute()
rospy.spin()
def __init__(self, grammar=None):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
output_keys=[])
with self:
self.userdata.asr_userSaid = ''
self.userdata.grammar_name = GRAMMAR_NAME
self.userdata.tts_text = None
self.userdata.tts_wait_before_speaking = None
self.userdata.tts_lang = None
# Listen
smach.StateMachine.add(
'listen_info',
ReadASR(),
transitions={'succeeded': 'process_command', 'aborted': 'aborted',
'preempted': 'preempted'})
# Prepare the information
smach.StateMachine.add(
"process_command",
ProcessCommand(),
transitions={'yes': 'succeeded', 'no': 'aborted', 'aborted':'repeat'})
# Ask for repeat
smach.StateMachine.add(
'repeat',
text_to_say("Excuse me, I don't understand you. Can you repeat?"),
transitions={'succeeded': 'listen_info', 'aborted': 'aborted',
'preempted': 'preempted'})
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
output_keys=['standard_error'])
with self:
# Preparation of the Ask Process
# Input Data: 'object_to_grasp'
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_lang = 'en_US'
smach.StateMachine.add('Ask_Person_Object',
text_to_say('I am giving you the object you asked'),
transitions={'succeeded':'Reach_Arm', 'preempted':'Reach_Arm', 'aborted':'Reach_Arm'})
# Reach the arm
self.userdata.manip_motion_to_play = 'give_object_right'
self.userdata.manip_time_to_play = 2.0
smach.StateMachine.add('Reach_Arm',
play_motion_sm(),
transitions={'succeeded':'Pre_Grasp', 'preempted':'Reach_Arm', 'aborted':'Reach_Arm'})
smach.StateMachine.add('Pre_Grasp',
move_hands_form(hand_pose_name='pre_grasp', hand_side='right'),
transitions={'succeeded':'Open_Hand', 'preempted':'Open_Hand', 'aborted':'Open_Hand'})
smach.StateMachine.add('Open_Hand',
move_hands_form(hand_pose_name='full_open', hand_side='right'),
transitions={'succeeded':'Home_position_end', 'preempted':'Home_position_end', 'aborted':'Home_position_end'})
smach.StateMachine.add('Home_position_end',
play_motion_sm("home", 4.0),
transitions={'succeeded':'succeeded', 'preempted':'preempted', 'aborted':'aborted'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error','in_learn_person'])
with self:
self.userdata.tts_wait_before_speaking=0
self.userdata.tts_text=None
self.userdata.tts_lang=None
self.userdata.standard_error='OK'
self.userdata.in_learn_person=1
self.userdata.type_movment = 'home'
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center","up"),
transitions={'succeeded': 'INTRO','aborted':'INTRO'})
smach.StateMachine.add('INTRO',
acknowledgment(type_movement='home', tts_text=START_FRASE),
transitions={'succeeded': 'Learn','aborted':'aborted'})
# it learns the person that we have to follow
smach.StateMachine.add('Learn',
LearnPerson(learn_face=True),
transitions={'succeeded': 'SM_STOP_LEARNING',
'aborted': 'aborted'})
smach.StateMachine.add('SM_STOP_LEARNING',
text_to_say(LETS_GO),
transitions={'succeeded': 'succeeded','aborted':'aborted'})
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=False),
transitions={'succeeded':'Move_head_Left', 'aborted':'aborted'})
smach.StateMachine.add('Move_head_Left',
move_head_form( head_left_right='mid_left', head_up_down='up'),
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',
move_head_form( head_left_right='mid_right', head_up_down='up'),
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',
move_head_form( head_left_right='center', head_up_down='up'),
transitions={'succeeded':'Wait_search_1', 'aborted':'Wait_search_1'})
def __init__(self, head_position=None):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted', 'end_searching'],
input_keys=[],
output_keys=['wave_position', 'wave_yaw_degree','standard_error'])
with self:
self.userdata.loop_iterations = 0
self.userdata.wave_position = None
self.userdata.wave_yaw_degree = None
smach.StateMachine.add(
'Move_head_prepare',
prepare_move_head(),
transitions={'succeeded': 'move_head', 'aborted': 'aborted',
'preempted': 'preempted', 'end_searching':'end_searching'})
smach.StateMachine.add(
'move_head',
move_head_form(head_up_down='normal'),
transitions={'succeeded': 'Say_Searching',
'preempted':'Say_Searching',
'aborted':'aborted'})
smach.StateMachine.add(
'Say_Searching',
text_to_say("I am searching, let's see"),
transitions={'succeeded':'wave_recognition', 'aborted':'wave_recognition', 'preempted':'wave_recognition'})
smach.StateMachine.add(
'wave_recognition',
WaveDetection(),
transitions={'succeeded': 'succeeded', 'aborted': 'Move_head_prepare',
'preempted': 'preempted'})
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, 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 __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, grammar = None):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['grammar_name'],
output_keys=['asr_userSaid','asr_userSaid_tags'])
with self:
self.userdata.asr_userSaid = ''
self.userdata.tts_lang = None
asr_userSaid_tags=''
smach.StateMachine.add('PrepareData',
prepareData(grammar),
transitions={'succeeded':'listen_info', 'aborted':'aborted'})
# Listen
smach.StateMachine.add(
'listen_info',
ListenToSM(),
transitions={'succeeded': 'prepare_info', 'aborted': 'aborted',
'preempted': 'preempted'})
# Prepare the information
smach.StateMachine.add(
"prepare_info",
prepare_say(),
transitions={'succeeded': 'say_info', 'aborted': 'aborted',
'preempted': 'preempted'})
# Repeat
smach.StateMachine.add(
'say_info',
text_to_say(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
def main():
rospy.init_node('say_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.tts_text = None
sm.userdata.tts_wait_before_speaking = None
sm.userdata.tts_lang = 'en_US'
smach.StateMachine.add('SaySM',
text_to_say("Hello!"),
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('saysm_test_introspection', sm,
'/SAY_TEST')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
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'),
transitions={
'succeeded': 'Move_head_Left',
'aborted': 'aborted'
})
smach.StateMachine.add('Move_head_Left',
move_head_form(head_left_right='mid_left',
head_up_down='normal'),
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',
move_head_form(head_left_right='mid_right',
head_up_down='normal'),
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',
move_head_form(head_left_right='center',
head_up_down='normal'),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
def __init__(self, tts=None, poi_name=None):
"""
Constructor for nav_to_coord.
"""
#Initialization of the SMACH State machine
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[
'nav_to_coord_goal', 'tts_text', 'tts_wait_before_speaking',
'tts_lang', 'nav_to_poi_name'
],
output_keys=[])
self.tts = tts
self.poi_name = poi_name
with self:
smach.StateMachine.add('say',
text_to_say(self.tts, wait=False),
transitions={
'succeeded': 'nav_to_poi',
'aborted': 'aborted'
})
smach.StateMachine.add('nav_to_poi',
nav_to_poi(self.poi_name),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
def main():
rospy.init_node('say_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.tts_text = None
sm.userdata.tts_wait_before_speaking = None
sm.userdata.tts_lang = 'en_US'
smach.StateMachine.add(
'SaySM',
text_to_say("Hello!"),
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(
'saysm_test_introspection', sm, '/SAY_TEST')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['object_to_grasp'],
output_keys=['standard_error'])
with self:
# Preparation of the Ask Process
# Input Data: 'object_to_grasp'
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_lang = 'en_US'
smach.StateMachine.add('Prepare_Ask_Person_Object',
prepare_ask_person_object(),
transitions={'succeeded':'Ask_Person_Object', 'preempted':'Ask_Person_Object', 'aborted':'Ask_Person_Object'})
smach.StateMachine.add('Ask_Person_Object',
text_to_say(),
transitions={'succeeded':'Reach_Arm', 'preempted':'Reach_Arm', 'aborted':'Reach_Arm'})
# Reach the arm
self.userdata.manip_motion_to_play = 'give_object_right'
smach.StateMachine.add('Reach_Arm',
play_motion_sm(),
transitions={'succeeded':'Pre_Grasp', 'preempted':'Reach_Arm', 'aborted':'Reach_Arm'})
smach.StateMachine.add('Pre_Grasp',
move_hands_form(hand_pose_name='pre_grasp', hand_side='right'),
transitions={'succeeded':'Full_Grasp', 'preempted':'Full_Grasp', 'aborted':'Full_Grasp'})
smach.StateMachine.add('Full_Grasp',
move_hands_form(hand_pose_name='grasp', hand_side='right'),
transitions={'succeeded':'succeeded', 'preempted':'preempted', 'aborted':'aborted'})
def __init__(self):
smach.StateMachine.__init__(self,
outcomes=['succeeded', 'preempted', 'aborted'],
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, frame_id='/map'):
"""
Constructor for nav_to_coord.
"""
#Initialization of the SMACH State machine
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['nav_to_coord_goal', 'nav_to_coord_frame'],
output_keys=['standard_error'])
with self:
smach.StateMachine.add('CreateNavGoal',
createNavGoal(frame_id),
transitions={
'succeeded': 'MoveRobot',
'aborted': 'aborted'
})
def move_res_cb(userdata, result_status, result):
if result_status != 3: # 3 == SUCCEEDED
if result_status == 4:
userdata.standard_error = "Aborted navigation goal (maybe we didn't get there?)"
rospy.loginfo(userdata.standard_error)
elif result_status == 5: # We havent got a rejected yet, maybe never happens
userdata.standard_error = "Rejected navigation goal (maybe the goal is outside of the map or in a obstacle?)"
rospy.loginfo(userdata.standard_error)
elif result_status == 2:
return 'preempted'
return 'aborted'
else:
userdata.standard_error = "OK"
return 'succeeded'
smach.StateMachine.add('MoveRobot',
SimpleActionState(
NAVIGATION_TOPIC_NAME,
MoveBaseAction,
goal_key='navigation_goal',
input_keys=['standard_error'],
output_keys=['standard_error'],
result_cb=move_res_cb),
transitions={
'succeeded': 'succeeded',
'aborted': 'Aborting_text',
'preempted': 'preempted'
})
smach.StateMachine.add(
'Aborting_text',
text_to_say('I cannot reach the location, sorry.'),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
def __init__(self, poi_name = None):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['point_to_poi_name'],
output_keys=['standard_error'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
print "POINT TO POI IN-SELF"
self.userdata.standard_error=''
self.userdata.point_to_coord_goal=[0.0,0.0,0.0]
smach.StateMachine.add('PrepareData',
prepareData(poi_name),
transitions={'succeeded':'translate_coord', 'aborted':'aborted'})
# We transform the poi to coordenades
smach.StateMachine.add('translate_coord',
translate_coord(),
transitions={'succeeded': 'get_pose', 'aborted': 'aborted', 'preempted': 'preempted'})
# We get current coodenades of robot in userdata 'current_robot_pose', 'current_robot_yaw', 'pose_current'
smach.StateMachine.add('get_pose',
GetPoseSubscribe(),
transitions={'succeeded': 'get_yaw', 'aborted': 'aborted', 'preempted': 'preempted'})
# We get current coodenades of robot in userdata 'current_robot_pose', 'current_robot_yaw', 'pose_current'
smach.StateMachine.add('get_yaw',
calculateYaw(), #output ['desired_angle']
transitions={'succeeded': 'prepareNav', 'aborted': 'aborted', 'preempted': 'preempted'})
# Prepares to turn
smach.StateMachine.add('prepareNav',
prepare_nav_to_coord(),
transitions={'succeeded': 'turn', 'aborted': 'aborted', 'preempted': 'preempted'})
# Turns
smach.StateMachine.add('turn',
nav_to_coord(),
transitions={'succeeded': 'point_to_coord', 'aborted': 'aborted', 'preempted': 'preempted'})
# Point the coordenades
smach.StateMachine.add('point_to_coord',
play_motion_sm('point_forward'),
transitions={'succeeded': 'Say_Pointing_Poi', 'aborted': 'aborted', 'preempted': 'preempted'})
# Say pointing
smach.StateMachine.add('Say_Pointing_Poi',
text_to_say('There, what I am pointing'),
transitions={'succeeded': 'home_position', 'aborted': 'aborted',
'preempted': 'preempted'})
# Return home position
smach.StateMachine.add('home_position',
play_motion_sm('home'),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted', 'preempted': 'preempted'})
def __init__(self, head_position=None, text_for_wave_searching="I am looking for you."):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
output_keys=['wave_position', 'wave_yaw_degree','standard_error'])
with self:
self.userdata.loop_iterations = 0
self.userdata.wave_position = None
self.userdata.wave_yaw_degree = None
self.userdata.standard_error = ''
smach.StateMachine('Dummy',
DummyStateMachine(),
transitions={'succeeded':'Say_follow'})
smach.StateMachine('Say_follow',
text_to_say('Follow Me Please, I will guide you to the person'),
transitions={'succeeded':'succeeded', 'aborted':'aborted'})
smach.StateMachine.add(
'Move_head_prepare',
prepare_move_head(head_position),
transitions={'succeeded': 'move_head', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'move_head',
move_head_form(head_up_down=head_position),
transitions={'succeeded': 'Say_Searching',
'preempted':'Say_Searching',
'aborted':'aborted'})
smach.StateMachine.add(
'Say_Searching',
text_to_say("text_for_wave_searching"),
transitions={'succeeded':'face_detection', 'aborted':'face_detection', 'preempted':'face_detection'})
#Hard-coded maximum time in order to detect wave
smach.StateMachine.add(
'face_detection',
detect_face(),
transitions={'succeeded': 'Say_Found', 'aborted': 'aborted', #TODO before it was Move_head_prepare
'preempted': 'preempted'})
smach.StateMachine.add(
'Say_Found',
text_to_say("Oh! I have found you finally."),
transitions={'succeeded':'succeeded', 'aborted':'aborted', 'preempted':'preempted'})
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):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted', 'end'],
input_keys=[],
output_keys=[])
with self:
self.userdata.pressed = False
self.userdata.press = False
self.userdata.tts_time_before_speaking = 0
self.userdata.tts_text = ""
self.userdata.tts_lang = ""
# Check if the button is pressed
smach.StateMachine.add(
'check_button',
emergency_button(),
transitions= {'succeeded':'check_button_pressed', 'aborted':'check_button', 'preempted':'preempted'})
#Cas 1 - Crec que es perdra tota l'estona si esta premut o no
# transitions= {'succeeded':'check_button_pressed', 'aborted':'aborted', 'preempted':'preempted'})
smach.StateMachine.add(
'check_button_pressed',
check_button_pressed(),
transitions= {'pressed':'say_pressed_button', 'unpressed':'say_not_pressed_button',
'succeeded':'sleep_state', 'preempted':'preempted'})
# Sleep
smach.StateMachine.add(
'sleep_state',
Sleeper(2),
transitions={'succeeded': 'check_button', 'aborted': 'check_button', 'preempted':'preempted'})
# Say button pressed
smach.StateMachine.add(
'say_pressed_button',
text_to_say("Oh you pressed my emergency button, feel free to check me around"),
transitions= {'succeeded':'check_button', 'aborted':'check_button', 'preempted':'preempted'})
# Say button is not pressed
smach.StateMachine.add(
'say_not_pressed_button',
text_to_say("Thank you for unpressing my emergency button. I'll recover my status shortly."),
transitions= {'succeeded':'end', 'aborted':'end', 'preempted':'preempted'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error','in_learn_person'])
with self:
self.userdata.tts_wait_before_speaking=0
self.userdata.tts_text=None
self.userdata.tts_lang=None
self.userdata.standard_error='OK'
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={'succeeded': 'SAY_LIFTIME',
'aborted': 'SAY_LIFTIME'})
smach.StateMachine.add('SAY_LIFTIME',
text_to_say(SAY_GO_MIDLE),
transitions={'succeeded': 'CONTROL_TIME',
'aborted': 'SAY_LIFTIME'})
smach.StateMachine.add('CONTROL_TIME',
control_time(),
transitions={'succeeded': 'READ_TRACKER_TOPIC',
'lif_time': 'INIT_VAR'})
smach.StateMachine.add('READ_TRACKER_TOPIC',
topic_reader(topic_name='/people_tracker_node/peopleSet',
topic_type=personArray,topic_time_out=60),
transitions={'succeeded':'SELECT_ID',
'aborted':'READ_TRACKER_TOPIC',
'preempted':'preempted'},
remapping={'topic_output_msg': 'tracking_msg'})
# it learns the person that we have to follow
smach.StateMachine.add('SELECT_ID',
select_ID(),
transitions={'succeeded': 'STOP_LEARNING',
'aborted': 'CONTROL_TIME'})
smach.StateMachine.add('STOP_LEARNING',
text_to_say("OK i have finsished"),
transitions={'succeeded': 'succeeded','aborted':'STOP_LEARNING'})
def main():
rospy.init_node('yes_no_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.grammar_name = None
sm.userdata.tts_text = None
sm.userdata.tts_wait_before_speaking = None
sm.userdata.tts_lang = None
# Load grammar yes/no
smach.StateMachine.add(
'ActivateASR_yesno',
ActivateASR("robocup/yes_no"),
transitions={'succeeded': 'DID_YOU_SAY', 'aborted': 'aborted', 'preempted': 'preempted'})
smach.StateMachine.add('DID_YOU_SAY',
text_to_say("ready"),
transitions={'succeeded': 'YesNoTest', 'aborted': 'aborted'})
smach.StateMachine.add('YesNoTest',
SayYesOrNoSM(),
transitions={'succeeded': 'SAY_SUCC', 'aborted': 'SAY_ABORTED'})
smach.StateMachine.add('SAY_SUCC',
text_to_say("SUCCEEDED"),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted'})
smach.StateMachine.add('SAY_ABORTED',
text_to_say("ABORTED"),
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(
'yes_no_introspection', sm, '/YES_NO_TEST')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted'],
output_keys=['standard_error'])
with self:
# Preparation of the Ask Process
# Input Data: 'object_to_grasp'
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_lang = 'en_US'
smach.StateMachine.add(
'Ask_Person_Object',
text_to_say('I am giving you the object you asked'),
transitions={
'succeeded': 'Reach_Arm',
'preempted': 'Reach_Arm',
'aborted': 'Reach_Arm'
})
# Reach the arm
self.userdata.manip_motion_to_play = 'give_object_right'
self.userdata.manip_time_to_play = 2.0
smach.StateMachine.add('Reach_Arm',
play_motion_sm(),
transitions={
'succeeded': 'Pre_Grasp',
'preempted': 'Reach_Arm',
'aborted': 'Reach_Arm'
})
smach.StateMachine.add('Pre_Grasp',
move_hands_form(hand_pose_name='pre_grasp',
hand_side='right'),
transitions={
'succeeded': 'Open_Hand',
'preempted': 'Open_Hand',
'aborted': 'Open_Hand'
})
smach.StateMachine.add('Open_Hand',
move_hands_form(hand_pose_name='full_open',
hand_side='right'),
transitions={
'succeeded': 'Home_position_end',
'preempted': 'Home_position_end',
'aborted': 'Home_position_end'
})
smach.StateMachine.add('Home_position_end',
play_motion_sm("home", 4.0),
transitions={
'succeeded': 'succeeded',
'preempted': 'preempted',
'aborted': 'aborted'
})
def __init__(self, frame_id="/map"):
"""
Constructor for nav_to_coord.
"""
# Initialization of the SMACH State machine
smach.StateMachine.__init__(
self,
outcomes=["succeeded", "preempted", "aborted"],
input_keys=["nav_to_coord_goal"],
output_keys=["standard_error"],
)
with self:
smach.StateMachine.add(
"CreateNavGoal", createNavGoal(frame_id), transitions={"succeeded": "MoveRobot", "aborted": "aborted"}
)
def move_res_cb(userdata, result_status, result):
if result_status != 3: # 3 == SUCCEEDED
rospy.logwarn("Error in NAV_TO_COORD: " + str(result))
if result_status == 4:
userdata.standard_error = "Aborted navigation goal (maybe we didn't get there?)"
rospy.loginfo(userdata.standard_error)
elif result_status == 5: # We havent got a rejected yet, maybe never happens
userdata.standard_error = (
"Rejected navigation goal (maybe the goal is outside of the map or in a obstacle?)"
)
rospy.loginfo(userdata.standard_error)
elif result_status == 2:
return "preempted"
return "aborted"
else:
userdata.standard_error = "OK"
return "succeeded"
smach.StateMachine.add(
"MoveRobot",
SimpleActionState(
NAVIGATION_TOPIC_NAME,
MoveBaseAction,
goal_key="navigation_goal",
input_keys=["standard_error"],
output_keys=["standard_error"],
result_cb=move_res_cb,
),
transitions={"succeeded": "succeeded", "aborted": "Aborting_text", "preempted": "preempted"},
)
smach.StateMachine.add(
"Aborting_text",
text_to_say("I cannot reach the location, sorry"),
transitions={"succeeded": "aborted", "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.standard_error = 'OK'
self.userdata.standard_error = 'OK'
self.userdata.in_learn_person = 1
self.userdata.word_to_listen = None
smach.StateMachine.add('INIT_FOLLOW',
FollowMeInit(),
transitions={
'succeeded': 'FOLLOW_ME_1rst',
'preempted': 'FOLLOW_ME_1rst',
'aborted': 'FOLLOW_ME_1rst'
})
smach.StateMachine.add('FOLLOW_ME_1rst',
follow_me_1st(),
transitions={
'succeeded': 'TEXT',
'aborted': 'aborted',
'preempted': 'TEXT',
'operator_say_out': 'FOLLOW_ME_3rd'
})
# in this state i will wait that the door it comes open
smach.StateMachine.add('TEXT',
text_to_say(LIFT_TEXT),
transitions={
'succeeded': 'FOLLOW_ME_2nd',
'aborted': 'aborted'
})
# im looking for go out of the lift
smach.StateMachine.add('FOLLOW_ME_2nd',
follow_me_2nd(),
transitions={
'succeeded': 'FOLLOW_ME_3rd',
'aborted': 'aborted'
})
smach.StateMachine.add('FOLLOW_ME_3rd',
follow_me_3rd(),
transitions={
'succeeded': 'succeeded',
'preempted': 'preempted',
'aborted': 'aborted'
})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['object_to_grasp'],
output_keys=['standard_error'])
with self:
# Preparation of the Ask Process
# Input Data: 'object_to_grasp'
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_lang = 'en_US'
smach.StateMachine.add('Prepare_Ask_Person_Object',
prepare_ask_person_object(),
transitions={
'succeeded': 'Ask_Person_Object',
'preempted': 'Ask_Person_Object',
'aborted': 'Ask_Person_Object'
})
smach.StateMachine.add('Ask_Person_Object',
text_to_say(),
transitions={
'succeeded': 'Reach_Arm',
'preempted': 'Reach_Arm',
'aborted': 'Reach_Arm'
})
# Reach the arm
self.userdata.manip_motion_to_play = 'give_object_right'
smach.StateMachine.add('Reach_Arm',
play_motion_sm(),
transitions={
'succeeded': 'Pre_Grasp',
'preempted': 'Reach_Arm',
'aborted': 'Reach_Arm'
})
smach.StateMachine.add('Pre_Grasp',
move_hands_form(hand_pose_name='pre_grasp',
hand_side='right'),
transitions={
'succeeded': 'Full_Grasp',
'preempted': 'Full_Grasp',
'aborted': 'Full_Grasp'
})
smach.StateMachine.add('Full_Grasp',
move_hands_form(hand_pose_name='grasp',
hand_side='right'),
transitions={
'succeeded': 'succeeded',
'preempted': 'preempted',
'aborted': 'aborted'
})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error'])
with self:
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_text = None
self.userdata.tts_lang = None
self.userdata.standar_error = "ok"
self.userdata.word_to_listen = None
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={
'succeeded': 'CONCURRENCE',
'aborted': 'aborted',
'preempted': 'preempted'
})
sm = smach.Concurrence(
outcomes=['DOOR_OPEN', 'OPERATOR', 'aborted'],
default_outcome='DOOR_OPEN',
input_keys=['word_to_listen'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm:
# it will finisheed with succeeded if it check a door
sm.add('CHECK_DOOR', look_for_elevator_door())
# here i have to listen if they say me to get out of the lift
sm.add('LISTEN_OPERATOR_FOR_EXIT',
ListenWordSM_Concurrent("go out"))
smach.StateMachine.add('CONCURRENCE',
sm,
transitions={
'OPERATOR': 'SAY_OUT',
'DOOR_OPEN': 'SAY_OUT',
'aborted': 'CONCURRENCE'
})
# it says i'm going out
smach.StateMachine.add('SAY_OUT',
text_to_say(SAY_OUT_FRASE),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['tts_text'],
output_keys=[])
with self:
smach.StateMachine.add('random_prepare',
random_speech_prepare(),
transitions={'succeeded':'say_random'})
smach.StateMachine.add('say_random',
text_to_say(),
transitions={'succeeded':'succeeded','aborted':'aborted'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error','in_learn_person'])
with self:
self.userdata.tts_wait_before_speaking=0
self.userdata.tts_text=None
self.userdata.tts_lang=None
self.userdata.head_left_right=None
self.userdata.head_up_down=None
self.userdata.standard_error='OK'
self.userdata.in_learn_person=1
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center","up"),
transitions={'succeeded': 'INTRO','aborted':'INTRO'})
smach.StateMachine.add('INTRO',
text_to_say(START_FRASE),
transitions={'succeeded': 'START_FOLLOWING_COME_CLOSER','aborted':'START_FOLLOWING_COME_CLOSER'})
smach.StateMachine.add('Listen',
ListenWordSM_Concurrent("follow me"),
transitions={'succeeded': 'START_FOLLOWING_COME_CLOSER',
'aborted': 'Listen'})
smach.StateMachine.add('START_FOLLOWING_COME_CLOSER',
text_to_say(START_FOLLOW_FRASE),
transitions={'succeeded': 'SM_LEARN_PERSON','aborted':'aborted'})
# it learns the person that we have to follow
smach.StateMachine.add('SM_LEARN_PERSON',
LearnPerson(learn_face=True),
transitions={'succeeded': 'SM_STOP_LEARNING',
'aborted': 'aborted'})
smach.StateMachine.add('SM_STOP_LEARNING',
text_to_say(LEARNED_PERSON_FRASE),
transitions={'succeeded': 'succeeded','aborted':'aborted'})
def __init__(self, type_movement="yes", tts_text=''):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['tts_text', 'type_movment'],
output_keys=['standard_error'])
self.type_movement = type_movement
self.tts_text = tts_text
with self:
self.userdata.tts_text = None
self.userdata.type_movment = None
self.userdata.tts_lang = None
self.userdata.tts_wait_before_speaking = 0
self.userdata.standard_error = 'OK'
self.userdata.manip_time_to_play = 30
self.userdata.skip_planning = False
smach.StateMachine.add('INIT_VAR',
init_var(self.type_movement, self.tts_text),
transitions={
'succeeded': 'PUT_MOVMENT',
'aborted': 'aborted',
'preempted': 'preempted'
})
sm = smach.Concurrence(
outcomes=['succeeded', 'preempted', 'aborted'],
default_outcome='succeeded',
input_keys=[
'tts_text', 'manip_motion_to_play', 'manip_time_to_play',
'tts_wait_before_speaking', 'tts_lang', 'skip_planning'
])
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_text = None
self.userdata.tts_lang = None
with sm:
sm.add('SAY', text_to_say())
sm.add('MOVE', play_motion_sm(skip_planning=False))
smach.StateMachine.add('PUT_MOVMENT',
sm,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
def __init__(self, grammar=None):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
output_keys=[])
with self:
self.userdata.asr_userSaid = ''
self.userdata.grammar_name = GRAMMAR_NAME
self.userdata.tts_text = None
self.userdata.tts_wait_before_speaking = None
self.userdata.tts_lang = None
# Activate the server
smach.StateMachine.add('ActivateASR',
ActivateASR(GRAMMAR_NAME),
transitions={
'succeeded': 'listen_info',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Listen
smach.StateMachine.add('listen_info',
ReadASR(),
transitions={
'succeeded': 'process_command',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Prepare the information
smach.StateMachine.add("process_command",
ProcessCommand(),
transitions={
'yes': 'succeeded',
'no': 'aborted',
'aborted': 'aborted'
})
# Ask for repeat
smach.StateMachine.add(
'repeat',
text_to_say(
"Excuse me, I don't understand you. Can you repeat?"),
transitions={
'succeeded': 'aborted',
'aborted': 'aborted',
'preempted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
output_keys=[],
input_keys=[])
with self:
#maybe i will have to learn again
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={
'succeeded': 'look_wave',
'aborted': 'aborted',
'preempted': 'preempted'
})
#maybe i will have to learn again
smach.StateMachine.add('look_wave',
WaveDetection(),
transitions={
'succeeded': 'say_hello',
'aborted': 'look_wave',
'preempted': 'preempted'
})
smach.StateMachine.add('say_hello',
text_to_say(Say, wait=False),
transitions={
'succeeded': 'wait_state',
'aborted': 'wait_state'
})
smach.StateMachine.add('play_motion_state',
play_motion_sm('wave', skip_planning=True),
transitions={
'succeeded': 'wait_state',
'preempted': 'wait_state',
'aborted': 'wait_state'
})
smach.StateMachine.add('wait_state',
waitstate(),
transitions={
'succeeded': 'look_wave',
'aborted': 'look_wave',
'preempted': 'look_wave'
})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['tts_text'],
output_keys=[])
with self:
smach.StateMachine.add('random_prepare',
random_speech_prepare(),
transitions={'succeeded': 'say_random'})
smach.StateMachine.add('say_random',
text_to_say(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
def main():
rospy.loginfo('follow_operator_TEST')
rospy.init_node('follow_operator_TEST')
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with sm:
sm.userdata.standard_error = 'OK'
# it prepare the name and the function for drope
smach.StateMachine.add('prepare_msg',
prepare_msg(),
transitions={
'succeeded': 'follow_operator',
'aborted': 'aborted',
'preempted': 'preempted'
})
# it call the drop_face state
smach.StateMachine.add('follow_operator',
FollowOperator(),
transitions={
'lost': 'say_lost',
'succeeded': 'follow_me_info'
})
sm.userdata.tts_text = "i have lose the person"
sm.userdata.tts_wait_before_speaking = 0
smach.StateMachine.add('say_lost',
text_to_say(),
transitions={
'succeeded': 'follow_me_info',
'aborted': 'follow_me_info'
})
smach.StateMachine.add('follow_me_info',
follow_operator_error(),
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('follow_operator_test', sm, '/FO_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', '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
smach.StateMachine.add(
'Say_Searching',
text_to_say('Right Now I am looking for the Ambulance.'),
transitions={
'succeeded': 'Concurrence',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Concurrence
sm_conc = smach.Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
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:
# 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': 'aborted',
'preempted': 'aborted'
})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'])
#rospy.init_node("SM_GPSR_ORDERS")
with self:
smach.StateMachine.add(
'Stand_Still',
text_to_say(text="Please don't move for a moment",
wait_before_speaking=0),
transitions={'succeeded': 'DataBase_and_Learn'})
smach.StateMachine.add('DataBase_and_Learn',
new_database_and_learn(
name_face='referee',
name_database='Soar'),
transitions={'succeeded': 'succeeded'})
def __init__(self, grammar=None):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['grammar_name'],
output_keys=['asr_userSaid', 'asr_userSaid_tags'])
with self:
self.userdata.asr_userSaid = ''
self.userdata.tts_lang = None
asr_userSaid_tags = ''
smach.StateMachine.add('PrepareData',
prepareData(grammar),
transitions={
'succeeded': 'listen_info',
'aborted': 'aborted'
})
# Listen
smach.StateMachine.add('listen_info',
ListenToSM(),
transitions={
'succeeded': 'prepare_info',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Prepare the information
smach.StateMachine.add("prepare_info",
prepare_say(),
transitions={
'succeeded': 'say_info',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Repeat
smach.StateMachine.add('say_info',
text_to_say(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
input_keys=['person_location'])
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
#It should be Speech Recognition: ListenTo(?)
smach.StateMachine.add('Prepare_Ask_Status',
prepare_tts('Are you Ok?'),
transitions={
'succeeded': 'Ask_Status',
'aborted': 'Ask_Status',
'preempted': 'Ask_Status'
})
smach.StateMachine.add('Ask_Status',
text_to_say(),
transitions={
'succeeded': 'Save_Info',
'aborted': 'Save_Info',
'preempted': 'Save_Info'
})
#TODO: Do we have to add an SayYesOrNoSM?
# smach.StateMachine.add(
# 'Yes_No',
# SayYesOrNoSM(),
# transitions={'succeeded':'Register_Position', 'aborted':'Register_Position', 'preempted':'Register_Position'})
#TODO: Actually Generate the PDF (TODO: Look for USB/Path...)
#Save_Info(): Saves the emergency info and generates a pdf file
#input_keys: emergency_location
smach.StateMachine.add(
'Save_Info',
DummyStateMachine(),
#GeneratePDF_State(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
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, GRAMMAR_NAME):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['delete_database'],
output_keys=['name_face', 'object_name'])
self.grammar_name = GRAMMAR_NAME
with self:
# Ask for order
smach.StateMachine.add('ask_order',
AskQuestionSM(
"what would you want to tell me",
self.grammar_name,
calibrate=True,
Bucle=False,
time_calibrate=10),
transitions={
'succeeded': 'succeeded',
'aborted': 'ask_order',
'preempted': 'preempted'
})
# Process the answer
smach.StateMachine.add(
'process_order',
process_order(),
transitions={
'succeeded': 'say_got_it',
'aborted': 'ask_order', # TODO before it was ask_order
'preempted': 'preempted'
})
# Say what he ask
smach.StateMachine.add('say_got_it',
text_to_say("I got it!"),
transitions={
'succeeded': 'succeeded',
'aborted': 'succeeded',
'preempted': 'preempted'
})