def __init__(self):
smach.StateMachine.__init__(self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['grammar_name'])
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.asr_userSaid=None
self.userdata.asr_userSaid_tags=None
self.userdata.objectName=""
self.userdata.objectOrientation=""
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={'succeeded': 'LISTEN_TO',
'aborted': 'aborted','preempted':'preempted'})
smach.StateMachine.add('LISTEN_TO',
ListenToSM(GRAMMAR_NAME),
transitions={'succeeded': 'PROCES_TAGS',
'aborted': 'CAN_YOU_REPEAT','preempted':'preempted'})
smach.StateMachine.add('CAN_YOU_REPEAT',
acknowledgment(tts_text=SAY_REPEAT,type_movement='no'),
transitions={'succeeded': 'LISTEN_TO',
'aborted': 'LISTEN_TO','preempted':'preempted'})
smach.StateMachine.add('PROCES_TAGS',
proces_Tags(),
transitions={'new_position': 'GET_POSE','finish':'succeeded',
'aborted': 'CAN_YOU_REPEAT','preempted':'preempted'})
smach.StateMachine.add('GET_POSE',
get_current_robot_pose(),
transitions={'succeeded': 'SAVE_POINT',
'aborted': 'GET_POSE',
'preempted':'preempted'})
#maybe the yaw of the robot we will have to change and say at your right you have....
smach.StateMachine.add('SAVE_POINT',
save_point(),
transitions={'succeeded': 'SAY_OK',
'aborted': 'aborted','preempted':'preempted'})
smach.StateMachine.add('SAY_OK',
acknowledgment(tts_text=SAY_OK,type_movement='yes'),
transitions={'succeeded': 'LISTEN_TO',
'aborted': 'LISTEN_TO','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'
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 main():
rospy.init_node('acknowledgment_hri')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.Skip_planning = False
smach.StateMachine.add(
'acknowledgment',
acknowledgment(
type_movement="yes",
tts_text="OK, i'm ready, do you wan't to be my friend"),
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('acknoledgemtn_test', sm,
'/acknowledgment')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
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('acknowledgment_hri')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.Skip_planning=False
smach.StateMachine.add(
'acknowledgment',
acknowledgment(type_movement="yes", tts_text="OK, i'm ready, do you wan't to be my friend"),
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(
'acknoledgemtn_test', sm, '/acknowledgment')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'], output_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.grammar_name = None
self.userdata.type_movment = None
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_lang = None
self.userdata.standard_error='OK'
self.userdata.object_name = ''
self.userdata.object_index = 0
# Activate the server
smach.StateMachine.add('ActivateASR',
ActivateASR(GRAMMAR_NAME),
transitions={'succeeded': 'ask_restaurant_order', 'aborted': 'aborted', 'preempted': 'preempted'})
# Ask Order
smach.StateMachine.add(
'ask_restaurant_order',
text_to_say("What would you like to order?"),
transitions={'succeeded': 'ReadASR', 'aborted': 'aborted',
'preempted': 'preempted'})
# Read from server
smach.StateMachine.add('ReadASR',
ReadASR(),
transitions={'succeeded': 'process_restaurant_order', 'aborted': 'aborted', 'preempted': 'preempted'})
# Listen Order
# smach.StateMachine.add(
# 'listen_restaurant_order',
# ListenToSM(GRAMMAR_NAME),
# transitions={'succeeded': 'process_restaurant_order', 'aborted': 'aborted',
# 'preempted': 'preempted'})
#
# Process order
smach.StateMachine.add(
'process_restaurant_order',
process_restaurant_order(),
transitions={'succeeded': 'ActivateASR_yesno', 'aborted': 'aborted'})
# Load grammar yes/no
smach.StateMachine.add(
'ActivateASR_yesno',
ActivateASR("robocup/yes_no"),
transitions={'succeeded': 'confirm_restaurant_order', 'aborted': 'aborted', 'preempted': 'preempted'})
# Confirm Order
smach.StateMachine.add(
'confirm_restaurant_order',
acknowledgment(type_movement="yes"),
transitions={'succeeded': 'yesno_restaurant_order', 'aborted': 'aborted',
'preempted': 'preempted'})
# Wait for yes or no
smach.StateMachine.add(
'yesno_restaurant_order',
SayYesOrNoSM(),
transitions={'succeeded': 'ok_im_going', 'aborted': 'aborted',
'preempted': 'preempted'})
# Ask for repeat the order
smach.StateMachine.add(
'repeat_restaurant_order',
acknowledgment(type_movement="no",tts_text="Excuse me, I don't understand you. Can you repeat your order?"),
transitions={'succeeded': 'ReadASR', 'aborted': 'aborted',
'preempted': 'preempted'})
# Confirm Order
smach.StateMachine.add(
'ok_im_going',
text_to_say("OK i'm going to service"),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
# Search for object information - It says where the object is, go to it and start object recognition -> RESTAURANT
# TODO: Add some messages in search object
# smach.StateMachine.add(
# 'search_food_order',
# SearchObjectSM(),
# transitions={'succeeded': 'go_to_storage', 'aborted': 'go_to_storage',
# 'preempted': 'preempted'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.nav_to_poi_name=''
self.userdata.manip_motion_to_play = ''
smach.StateMachine.add(
'play_motion_state',
play_motion_sm('home'),
transitions={'succeeded': 'say_start_basic_functionalities',
'preempted':'say_start_basic_functionalities',
'aborted':'say_start_basic_functionalities'})
# Say Start basic Functionalities
smach.StateMachine.add(
'say_start_basic_functionalities',
text_to_say("I'm ready to start Basic Functionalities"),
transitions={'succeeded': 'say_going_pick_place', 'aborted': 'say_going_pick_place'})
# Say Go Pick and Place
smach.StateMachine.add(
'say_going_pick_place',
text_to_say("I'm going to the Pick and Place location"),
transitions={'succeeded': 'go_pick_and_place', 'aborted': 'go_pick_and_place'})
# Go to pick and place
smach.StateMachine.add(
'go_pick_and_place',
Go_Poi_Listen_Word('init_pick_and_place', SENTENCE_STOP),
#Go_Poi_Listen_Word('point_room_one', SENTENCE_STOP),
#transitions={'succeeded': 'say_going_avoid', 'aborted': 'confirm_stop_pick_place',
#'preempted': 'preempted'})
transitions={'succeeded':'pick_timer', 'aborted': 'confirm_stop_pick_place',
'preempted': 'preempted'})
# The robot wait for "move" to move to the poi
smach.StateMachine.add(
'confirm_stop_pick_place',
acknowledgment("yes", "Okey, I stop"),
transitions={'succeeded': 'wait_pick_and_place', 'aborted': 'wait_pick_and_place',
'preempted': 'preempted'})
smach.StateMachine.add(
'wait_pick_and_place',
ListenWordSM_Concurrent(SENTENCE_GO),
transitions={'succeeded': 'confirm_move_pick_place', 'aborted': 'wait_pick_and_place',
'preempted': 'preempted'})
smach.StateMachine.add(
'confirm_move_pick_place',
acknowledgment("yes", "Okey, I move"),
transitions={'succeeded': 'say_going_pick_place', 'aborted': 'say_going_pick_place',
'preempted': 'preempted'})
# Do pick and place + TimeOut
STATES = [PickPlaceSM(), TimeOut(180)]
STATE_NAMES = ["do_pick_and_place", "time_pick"]
outcome_map = {'succeeded': {"do_pick_and_place": 'succeeded'}, 'aborted': {"time_pick": 'succeeded'}}
smach.StateMachine.add(
"pick_timer",
ConcurrenceRobocup(states=STATES, state_names=STATE_NAMES, outcome_map=outcome_map),
transitions={'succeeded': 'say_going_avoid', 'aborted': "timeout_pick_and_place"})
# Say TimeOut
smach.StateMachine.add(
'timeout_pick_and_place',
text_to_say("The time for Pick and Place is finish"),
transitions={'succeeded': 'say_going_avoid', 'aborted': 'say_going_avoid',
'preempted': 'preempted'})
# smach.StateMachine.add(
# 'do_pick_and_place',
# PickPlaceSM(),
# transitions={'succeeded': 'say_going_avoid', 'aborted': 'say_going_avoid',
# 'preempted': 'preempted'})
# Say Go Avoid that
smach.StateMachine.add(
'say_going_avoid',
text_to_say("I'm going to the Avoid that Area"),
transitions={'succeeded': 'go_avoid_that', 'aborted': 'go_avoid_that'})
# Go to avoid that
smach.StateMachine.add(
'go_avoid_that',
Go_Poi_Listen_Word('init_avoid_that', SENTENCE_STOP),
#Go_Poi_Listen_Word('point_room_two', SENTENCE_STOP),
#transitions={'succeeded': 'say_going_what_say', 'aborted': 'confirm_stop_avoid_that',
#'preempted': 'preempted'})
transitions={'succeeded': 'avoid_timer', 'aborted': 'confirm_stop_avoid_that',
'preempted': 'preempted'})
# The robot wait for "move" to move to the poi
smach.StateMachine.add(
'confirm_stop_avoid_that',
acknowledgment("yes", "Okey, I stop"),
transitions={'succeeded': 'wait_avoid_that', 'aborted': 'wait_avoid_that',
'preempted': 'preempted'})
smach.StateMachine.add(
'wait_avoid_that',
ListenWordSM_Concurrent(SENTENCE_GO),
transitions={'succeeded': 'confirm_move_avoid_that', 'aborted': 'wait_avoid_that',
'preempted': 'preempted'})
smach.StateMachine.add(
'confirm_move_avoid_that',
acknowledgment("yes", "Okey, I move"),
transitions={'succeeded': 'say_going_avoid', 'aborted': 'say_going_avoid',
'preempted': 'preempted'})
# Do avoid that + TimeOut
STATES = [Avoid_That(), TimeOut(180)]
STATE_NAMES = ["do_avoid_that", "time_avoid"]
outcome_map = {'succeeded': {"do_avoid_that": 'succeeded'}, 'aborted': {"time_avoid": 'succeeded'}}
smach.StateMachine.add(
"avoid_timer",
ConcurrenceRobocup(states=STATES, state_names=STATE_NAMES, outcome_map=outcome_map),
transitions={'succeeded': 'say_going_what_say', 'aborted': "timeout_avoid_that"})
# Say TimeOut
smach.StateMachine.add(
'timeout_avoid_that',
text_to_say("The time for Avoid That is finish"),
transitions={'succeeded': 'say_going_what_say', 'aborted': 'say_going_what_say',
'preempted': 'preempted'})
# Do avoid that
# smach.StateMachine.add(
# 'do_avoid_that',
# Avoid_That(),
# transitions={'succeeded': 'say_going_what_say', 'aborted': 'say_going_what_say',
# 'preempted': 'preempted'})
# Say Go What did you say
smach.StateMachine.add(
'say_going_what_say',
text_to_say("I'm going to the What did you say Location"),
transitions={'succeeded': 'go_what_did_you_say', 'aborted': 'go_what_did_you_say'})
# Go to what did you say
smach.StateMachine.add(
'go_what_did_you_say',
Go_Poi_Listen_Word('init_what_say', SENTENCE_STOP),
#Go_Poi_Listen_Word('point_room_three', SENTENCE_STOP),
#transitions={'succeeded': 'say_finish_basic_functionalities', 'aborted': 'confirm_stop_what_say',
#'preempted': 'preempted'})
transitions={'succeeded': 'what_say_timer', 'aborted': 'confirm_stop_what_say',
'preempted': 'preempted'})
# The robot wait for "move" to move to the poi
smach.StateMachine.add(
'confirm_stop_what_say',
acknowledgment("yes", "Okey, I stop"),
transitions={'succeeded': 'wait_what_did_you_say', 'aborted': 'wait_what_did_you_say',
'preempted': 'preempted'})
smach.StateMachine.add(
'wait_what_did_you_say',
ListenWordSM_Concurrent(SENTENCE_GO),
transitions={'succeeded': 'confirm_move_what_say', 'aborted': 'wait_what_did_you_say',
'preempted': 'preempted'})
smach.StateMachine.add(
'confirm_move_what_say',
acknowledgment("yes", "Okey, I move"),
transitions={'succeeded': 'say_going_what_say', 'aborted': 'say_going_what_say',
'preempted': 'preempted'})
# Do what did you say + TimeOut
STATES = [WhatSaySM(), TimeOut(180)]
STATE_NAMES = ["do_what_did_you_say", "time_what_say"]
outcome_map = {'succeeded': {"do_what_did_you_say": 'succeeded'}, 'aborted': {"time_what_say": 'succeeded'}}
smach.StateMachine.add(
"what_say_timer",
ConcurrenceRobocup(states=STATES, state_names=STATE_NAMES, outcome_map=outcome_map),
transitions={'succeeded': 'say_going_what_say', 'aborted': "timeout_what_say"})
# Say TimeOut
smach.StateMachine.add(
'timeout_what_say',
text_to_say("The time for What did you say is finish"),
transitions={'succeeded': 'say_finish_basic_functionalities', 'aborted': 'say_finish_basic_functionalities',
'preempted': 'preempted'})
# Do what did you say
# smach.StateMachine.add(
# 'do_what_did_you_say',
# WhatSaySM(),
# transitions={'succeeded': 'say_finish_basic_functionalities', 'aborted': 'say_finish_basic_functionalities',
# 'preempted': 'preempted'})
# Say Finish basic Functionalities
smach.StateMachine.add(
'say_finish_basic_functionalities',
text_to_say("I finished Basic Functionalities"),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
output_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.grammar_name = None
self.userdata.type_movment = None
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_lang = None
self.userdata.standard_error = 'OK'
self.userdata.object_name = ''
self.userdata.object_index = 0
# Activate the server
smach.StateMachine.add('ActivateASR',
ActivateASR(GRAMMAR_NAME),
transitions={
'succeeded': 'ask_restaurant_order',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Ask Order
smach.StateMachine.add(
'ask_restaurant_order',
text_to_say("What would you like to order?"),
transitions={
'succeeded': 'ReadASR',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Read from server
smach.StateMachine.add('ReadASR',
ReadASR(),
transitions={
'succeeded': 'process_restaurant_order',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Listen Order
# smach.StateMachine.add(
# 'listen_restaurant_order',
# ListenToSM(GRAMMAR_NAME),
# transitions={'succeeded': 'process_restaurant_order', 'aborted': 'aborted',
# 'preempted': 'preempted'})
#
# Process order
smach.StateMachine.add('process_restaurant_order',
process_restaurant_order(),
transitions={
'succeeded': 'ActivateASR_yesno',
'aborted': 'aborted'
})
# Load grammar yes/no
smach.StateMachine.add('ActivateASR_yesno',
ActivateASR("robocup/yes_no"),
transitions={
'succeeded': 'confirm_restaurant_order',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Confirm Order
smach.StateMachine.add('confirm_restaurant_order',
acknowledgment(type_movement="yes"),
transitions={
'succeeded': 'yesno_restaurant_order',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Wait for yes or no
smach.StateMachine.add('yesno_restaurant_order',
SayYesOrNoSM(),
transitions={
'succeeded': 'ok_im_going',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Ask for repeat the order
smach.StateMachine.add(
'repeat_restaurant_order',
acknowledgment(
type_movement="no",
tts_text=
"Excuse me, I don't understand you. Can you repeat your order?"
),
transitions={
'succeeded': 'ReadASR',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Confirm Order
smach.StateMachine.add('ok_im_going',
text_to_say("OK i'm going to service"),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Search for object information - It says where the object is, go to it and start object recognition -> RESTAURANT
# TODO: Add some messages in search object
# smach.StateMachine.add(
# 'search_food_order',
# SearchObjectSM(),
# transitions={'succeeded': 'go_to_storage', 'aborted': 'go_to_storage',
# 'preempted': 'preempted'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.nav_to_poi_name=''
self.userdata.manip_motion_to_play = ''
# Say Start basic Functionalities
smach.StateMachine.add(
'say_start_basic_functionalities',
text_to_say("I'm ready to start Basic Functionalities"),
transitions={'succeeded': 'say_going_pick_place', 'aborted': 'play_motion_state'}) # TODO before it was say_going_pick_place
# Say Go Pick and Place
smach.StateMachine.add(
'say_going_pick_place',
text_to_say("I'm going to the Pick and Place location"),
transitions={'succeeded': 'enter_start_door', 'aborted': 'enter_start_door'})
# Cross start door and go to intermediate point
smach.StateMachine.add(
'enter_start_door',
EnterRoomSM('kitchen_table'),
transitions={'succeeded': 'pick_timer', 'aborted': 'aborted',
'preempted': 'preempted'})
# Do pick and place + TimeOut
STATES = [PickPlaceSM()]
STATE_NAMES = ["do_pick_and_place"]
smach.StateMachine.add(
"pick_timer",
ConcurrenceTime(states=STATES, state_name=STATE_NAMES, timeout=180),
transitions={'succeeded': 'say_going_avoid',
'aborted':'say_going_avoid',
'time_ends': "timeout_pick_and_place"})
#
# smach.StateMachine.add(
# 'do_pick_and_place',
# PickPlaceSM(),
# transitions={'succeeded': 'play_motion_state_2', 'aborted': 'play_motion_state_2',
# 'preempted': 'preempted'})
# Say TimeOut
smach.StateMachine.add(
'timeout_pick_and_place',
text_to_say("The time for Pick and Place is finish"),
transitions={'succeeded': 'play_motion_state', 'aborted': 'play_motion_state',
'preempted': 'preempted'})
# Home position just in case that we are in pick position
smach.StateMachine.add(
'play_motion_state',
play_motion_sm('home', skip_planning=True),
transitions={'succeeded': 'say_going_avoid',
'preempted':'say_going_avoid',
'aborted':'play_motion_state'})
# Say Go Avoid that
smach.StateMachine.add(
'say_going_avoid',
text_to_say("I'm going to the Avoid that Area"),
transitions={'succeeded': 'go_avoid_that', 'aborted': 'go_avoid_that'})
# Go to avoid that
smach.StateMachine.add(
'go_avoid_that',
nav_to_poi('avoid_that'),
transitions={'succeeded': 'wait_avoid_that', 'aborted': 'go_avoid_that',
'preempted': 'preempted'})
# Wait for Go command
smach.StateMachine.add(
'wait_avoid_that',
ListenWordSM_Concurrent(SENTENCE_GO),
transitions={'succeeded': 'confirm_move_avoid_that', 'aborted': 'wait_avoid_that',
'preempted': 'preempted'})
smach.StateMachine.add(
'confirm_move_avoid_that',
acknowledgment("yes", "Okey, I move"),
transitions={'succeeded': 'avoid_timer', 'aborted': 'say_going_avoid',
'preempted': 'preempted'})
# Do avoid that + TimeOut
STATES = [Avoid_That()]
STATE_NAMES = ["do_avoid_that"]
smach.StateMachine.add(
"avoid_timer",
ConcurrenceTime(states=STATES, state_name=STATE_NAMES, timeout=180),
transitions={'succeeded': 'say_going_what_say',
'aborted':'say_going_what_say',
'time_ends': "timeout_avoid_that"})
# Say TimeOut
smach.StateMachine.add(
'timeout_avoid_that',
text_to_say("The time for Avoid That is finish"),
transitions={'succeeded': 'say_going_what_say', 'aborted': 'say_going_what_say',
'preempted': 'preempted'})
# Do avoid that
# smach.StateMachine.add(
# 'do_avoid_that',
# Avoid_That(),
# transitions={'succeeded': 'say_going_what_say', 'aborted': 'say_going_what_say',
# 'preempted': 'preempted'})
# Say Go What did you say
smach.StateMachine.add(
'say_going_what_say',
text_to_say("I'm ready to start the What did you say test"),
transitions={'succeeded': 'wait_what_did_you_say', 'aborted': 'wait_what_did_you_say'})
# Wait for Go command
smach.StateMachine.add(
'wait_what_did_you_say',
ListenWordSM_Concurrent(SENTENCE_GO),
transitions={'succeeded': 'confirm_move_what_say', 'aborted': 'wait_what_did_you_say',
'preempted': 'preempted'})
smach.StateMachine.add(
'confirm_move_what_say',
acknowledgment("yes", "Okey, I go"),
transitions={'succeeded': 'what_say_timer', 'aborted': 'what_say_timer',
'preempted': 'preempted'})
# Do what did you say + TimeOut
STATES = [WhatSaySM()]
STATE_NAMES = ["do_what_did_you_say"]
smach.StateMachine.add(
"what_say_timer",
ConcurrenceTime(states=STATES, state_name=STATE_NAMES, timeout=180),
transitions={'succeeded': 'go_to_door',
'aborted':'go_to_door',
'time_ends': "timeout_what_say"})
# Say TimeOut
smach.StateMachine.add(
'timeout_what_say',
text_to_say("The time for What did you say is finish"),
transitions={'succeeded': 'go_to_door', 'aborted': 'go_to_door',
'preempted': 'preempted'})
# Do what did you say
# smach.StateMachine.add(
# 'do_what_did_you_say',
# WhatSaySM(),
# transitions={'succeeded': 'say_finish_basic_functionalities', 'aborted': 'say_finish_basic_functionalities',
# 'preempted': 'preempted'})
# Go to the exit door
smach.StateMachine.add(
'go_to_door',
nav_to_poi('door_exit'),
transitions={'succeeded': 'go_to_exit', 'aborted': 'go_to_exit',
'preempted': 'preempted'})
# Go to the exit door
smach.StateMachine.add(
'go_to_exit',
nav_to_poi('door_B'),
transitions={'succeeded': 'say_finish_basic_functionalities', 'aborted': 'say_finish_basic_functionalities',
'preempted': 'preempted'})
# Say Finish basic Functionalities
smach.StateMachine.add(
'say_finish_basic_functionalities',
text_to_say("I finished Basic Functionalities"),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['grammar_name'])
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.asr_userSaid = None
self.userdata.asr_userSaid_tags = None
self.userdata.objectName = ""
self.userdata.objectOrientation = ""
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={
'succeeded': 'feedback_listen',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('feedback_listen',
text_to_say("i am listening"),
transitions={
'succeeded': 'LISTEN_TO',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add(
'LISTEN_TO',
ListenToSM(GRAMMAR_NAME),
transitions={
'succeeded': 'PROCES_TAGS', # 'PROCES_TAGS',
'aborted': 'CAN_YOU_REPEAT',
'preempted': 'preempted'
})
smach.StateMachine.add('CAN_YOU_REPEAT',
acknowledgment(tts_text=SAY_REPEAT,
type_movement='no'),
transitions={
'succeeded': 'LISTEN_TO',
'aborted': 'LISTEN_TO',
'preempted': 'preempted'
})
smach.StateMachine.add('PROCES_TAGS',
proces_Tags(),
transitions={
'new_position': 'feedback_repead',
'finish': 'succeeded',
'aborted': 'CAN_YOU_REPEAT',
'preempted': 'preempted'
})
smach.StateMachine.add('feedback_repead',
text_to_say(),
transitions={
'succeeded': 'GET_POSE',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('GET_POSE',
get_current_robot_pose_mapping(),
transitions={
'succeeded': 'SAVE_POINT',
'aborted': 'GET_POSE',
'preempted': 'preempted'
})
#maybe the yaw of the robot we will have to change and say at your right you have....
smach.StateMachine.add('SAVE_POINT',
save_point(),
transitions={
'succeeded': 'SAY_OK',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('SAY_OK',
acknowledgment(tts_text=SAY_OK,
type_movement='yes'),
transitions={
'succeeded': 'LISTEN_TO',
'aborted': 'LISTEN_TO',
'preempted': 'preempted'
})
