def main():
rospy.init_node('sm_test_listen_to')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
sm.userdata.grammar_name = "JungleParty"
smach.StateMachine.add('ListenToTest',
ListenToSM("JungleParty"),
transitions={
'succeeded': 'succe',
'aborted': 'aborted'
})
smach.StateMachine.add('succe',
Speaking_cb(),
transitions={'succeeded': 'succeeded'})
# 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('listen_to_test_introspection', sm,
'/LISTEN_TO_TEST')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def __init__(self, word=None):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['word_to_listen'],
output_keys=[])
with self:
self.userdata.listen_word = None
self.userdata.grammar_name = "robocup/listenword"
smach.StateMachine.add('PrepareData',
prepareData(word),
transitions={
'succeeded': 'listen_word',
'aborted': 'aborted'
})
# Listen the word
smach.StateMachine.add('listen_word',
ListenToSM(),
transitions={
'succeeded': 'checkData',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Check information
smach.StateMachine.add('checkData',
checkData(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
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,
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',
text_to_say(SAY_REPEAT),
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'})
# i have understand i have to ask
smach.StateMachine.add('feedback_repead',
text_to_say(),
transitions={'succeeded': 'confirm_order',
'aborted': 'LISTEN_TO','preempted':'preempted'})
smach.StateMachine.add('confirm_order',
SayYesOrNoSM_2(),
transitions={'succeeded': 'GET_POSE',
'aborted': 'CAN_YOU_REPEAT','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',
text_to_say(SAY_OK),
transitions={'succeeded': 'LISTEN_TO',
'aborted': 'LISTEN_TO','preempted':'preempted'})
def __init__(self):
smach.StateMachine.__init__(
self, ['succeeded', 'preempted', 'aborted'],
input_keys=['person_location', 'person_location_coord'])
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
smach.StateMachine.add(
'Ask_Question',
text_to_say(text='What would you like me to bring?'),
transitions={
'succeeded': 'Listen_Question',
'aborted': 'Ask_Question',
'preempted': 'Listen_Question'
})
# Ask for the object to bring to the person
# Output:
# - string 'userSaid'
# - actiontag[] 'asr_userSaid_tags'
# TODO: grammar for the Emergency Situation -- Get_Person_Desired_Object
smach.StateMachine.add('Listen_Question',
ListenToSM(grammar='robocup/emergency'),
transitions={
'succeeded': 'Process_Tags',
'aborted': 'Ask_Question',
'preempted': 'Ask_Question'
})
# Get the output from AskQuestionSM, process it, and search in the yaml file for the location of the object asked
# Input keys: actiontag[] 'asr_userSaid_tags'
# Output keys: object
smach.StateMachine.add('Process_Tags',
Process_Tags(),
transitions={
'succeeded': 'Say_go_Kitchen',
'aborted': 'Ask_Question',
'aborted': 'Ask_Question'
})
smach.StateMachine.add(
'Say_go_Kitchen',
text_to_say(
'I am Going to the Kitchen for an object, Stay Here until I give you the object'
),
transitions={
'succeeded': 'Go_To_Object_Place',
'aborted': 'Go_To_Object_Place',
'aborted': 'Go_To_Object_Place'
})
smach.StateMachine.add('Go_To_Object_Place',
nav_to_poi('kitchen'),
transitions={
'succeeded': 'Grasp_fail_Ask_Person',
'aborted': 'Grasp_fail_Ask_Person',
'preempted': 'Grasp_fail_Ask_Person'
})
self.userdata.time_grasp = 0.0
smach.StateMachine.add('Grasping_with_timeout',
grasping_with_timeout(),
transitions={
'succeeded': 'Prepare_Go_To_Person',
'time_out': 'Grasp_fail_Ask_Person'
})
# sm_conc = smach.Concurrence(outcomes=['succeeded', 'time_out'],
# default_outcome='succeeded',
# input_keys=['object_to_grasp, time_grasp'],
# child_termination_cb = child_term_cb,
# outcome_cb = out_cb)
#
# with sm_conc:
# sm_conc.add(
# 'Find_and_grab_object',
# #Find_and_grab_object(),
# DummyStateMachine())
# sm_conc.add(
# 'Time_State',
# Time_State())
#
# smach.StateMachine.add('GRASP_CONCURRENCE',
# sm_conc,
# transitions={'succeeded':'Prepare_Go_To_Person',
# 'time_out':'Grasp_fail_Ask_Person'})
#Find Object + Grab Object SM
# smach.StateMachine.add(
# 'Find_and_grab_object',
# #Find_and_grab_object(),
# DummyStateMachine(),
# transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'Grasp_fail_Ask_Person'})
smach.StateMachine.add('Grasp_fail_Ask_Person',
ask_give_object_grasping(),
transitions={
'succeeded': 'Rest_arm',
'aborted': 'Rest_arm',
'preempted': 'Rest_arm'
})
smach.StateMachine.add('Rest_arm',
play_motion_sm('rest_object_right'),
transitions={
'succeeded': 'Prepare_Go_To_Person',
'aborted': 'Prepare_Go_To_Person',
'preempted': 'Prepare_Go_To_Person'
})
#Go to person
smach.StateMachine.add('Prepare_Go_To_Person',
prepare_poi_person_emergency(),
transitions={
'succeeded': 'Go_To_Person',
'aborted': 'Go_To_Person',
'preempted': 'Go_To_Person'
})
#TODO: POI For Person in Emergency -- From SearchPeople SM -
smach.StateMachine.add(
'Go_To_Person',
#DummyStateMachine(),
#nav_to_poi(),
nav_to_coord('/map'),
transitions={
'succeeded': 'Say_Give_Object',
'aborted': 'Say_Give_Object',
'preempted': 'Say_Give_Object'
})
smach.StateMachine.add(
'Say_Give_Object',
text_to_say('I am going to give you the Object you want.'),
transitions={
'succeeded': 'Give_object_arm',
'aborted': 'Give_object_arm',
'preempted': 'Give_object_arm'
})
smach.StateMachine.add('Give_object_arm',
play_motion_sm('give_object_right'),
transitions={
'succeeded': 'Give_Object',
'aborted': 'Give_Object',
'preempted': 'Give_Object'
})
#Give the grabbed object to the person
smach.StateMachine.add(
'Give_Object',
#DummyStateMachine(),
move_hands_form(hand_pose_name='pre_grasp', hand_side='right'),
transitions={
'succeeded': 'Give_Object_2',
'aborted': 'Give_Object',
'preempted': 'Give_Object'
})
smach.StateMachine.add(
'Give_Object_2',
#DummyStateMachine(),
move_hands_form(hand_pose_name='full_open', hand_side='right'),
transitions={
'succeeded': 'Say_Rescue_stay',
'aborted': 'Give_Object_2',
'preempted': 'Give_Object_2'
})
smach.StateMachine.add(
'Say_Rescue_stay',
text_to_say(
'Please Stay here I am going to call for the Ambulance'),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'aborted': 'preempted'
})
def __init__(self):
smach.StateMachine.__init__(
self, ['succeeded', 'preempted', 'aborted'],
input_keys=['person_location', 'person_location_coord'])
pose_place = PoseStamped()
pose_place.header.frame_id = '/base_link'
pose_place.pose.position.x = 0.0
pose_place.pose.position.y = 0.0
pose_place.pose.position.z = 1.0
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
self.userdata.object_failed = False
self.userdata.object_name = None
smach.StateMachine.add(
'Ask_Question',
text_to_say(text='What would you like me to bring?'),
transitions={
'succeeded': 'Listen_Question',
'aborted': 'Ask_Question',
'preempted': 'Listen_Question'
})
smach.StateMachine.add('Listen_Question',
ListenToSM(grammar='robocup/emergency'),
transitions={
'succeeded': 'Process_Tags',
'aborted': 'Ask_Question',
'preempted': 'Ask_Question'
})
# Get the output from AskQuestionSM, process it, and search in the yaml file for the location of the object asked
# Input keys: actiontag[] 'asr_userSaid_tags'
# Output keys: object
smach.StateMachine.add('Process_Tags',
Process_Tags(),
transitions={
'succeeded': 'Search_Object',
'aborted': 'Ask_Question',
'aborted': 'Ask_Question'
})
smach.StateMachine.add(
'Search_Object',
object_detection_and_grasping_sm(),
transitions={
'succeeded': 'Say_return_Person',
'fail_object_detection': 'Grasp_failed_prepare',
'fail_object_grasping': 'Grasp_failed_prepare',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'object_name': 'object_to_grasp'})
smach.StateMachine.add(
'Grasp_failed_prepare',
Fail_Detection_Grasping(),
transitions={'succeeded': 'Grasp_fail_Ask_Person'})
# TODO: Saying to where to robot is heading.
# smach.StateMachine.add(
# 'Say_go_Place',
# text_to_say('I am Going to the Kitchen for an object, Stay Here until I give you the object'),
# transitions={'succeeded':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place', 'aborted':'Go_To_Object_Place'})
# smach.StateMachine.add(
# 'Go_To_Object_Place',
# nav_to_poi(),
# transitions={'succeeded':'Say_got_to_Kitchen', 'aborted':'Grasp_fail_Ask_Person', 'preempted':'Grasp_fail_Ask_Person'})
# smach.StateMachine.add(
# 'Say_got_to_Kitchen',
# text_to_say('I am in the Kitchen, I am going to grasp fail ask person'),
# transitions={'succeeded':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person', 'aborted':'Grasp_fail_Ask_Person'})
#
# self.userdata.time_grasp = 0.0
# smach.StateMachine.add('Grasping_with_timeout',
# grasping_with_timeout(),
# transitions={'succeeded':'Prepare_Go_To_Person', 'time_out':'Grasp_fail_Ask_Person'})
smach.StateMachine.add('Grasp_fail_Ask_Person',
ask_give_object_grasping(),
transitions={
'succeeded': 'Rest_arm',
'aborted': 'Rest_arm',
'preempted': 'Rest_arm'
})
smach.StateMachine.add('Rest_arm',
play_motion_sm('rest_object_right'),
transitions={
'succeeded': 'Say_return_Person',
'aborted': 'Say_return_Person',
'preempted': 'Say_return_Person'
})
smach.StateMachine.add(
'Say_return_Person',
text_to_say('I am preparing to go back to the person'),
transitions={
'succeeded': 'Prepare_Go_To_Person',
'aborted': 'Prepare_Go_To_Person',
'aborted': 'Prepare_Go_To_Person'
})
#Go to person
smach.StateMachine.add('Prepare_Go_To_Person',
prepare_poi_person_emergency(),
transitions={
'succeeded': 'Go_To_Person',
'aborted': 'Go_To_Person',
'preempted': 'Go_To_Person'
})
#TODO: POI For Person in Emergency -- From SearchPeople SM -
smach.StateMachine.add('Go_To_Person',
nav_to_coord('/map'),
transitions={
'succeeded': 'Say_Give_Object',
'aborted': 'Say_Give_Object',
'preempted': 'Say_Give_Object'
})
smach.StateMachine.add(
'Say_Give_Object',
text_to_say('I am going to give you the Object you asked.'),
transitions={
'succeeded': 'Select_next_state_grasping',
'aborted': 'Select_next_state_grasping',
'preempted': 'Select_next_state_grasping'
})
smach.StateMachine.add('Select_next_state_grasping',
Select_Grasp_Next_State(),
transitions={
'state_failed': 'Give_object_arm',
'state_succeeded': 'Give_object_both'
})
smach.StateMachine.add('Give_object_both',
place_object_sm(pose_place),
transitions={
'succeeded': 'Say_Rescue_stay',
'aborted': 'Say_Rescue_stay',
'preempted': 'preempted'
})
smach.StateMachine.add('Give_object_arm',
play_motion_sm('give_object_right'),
transitions={
'succeeded': 'Give_Object',
'aborted': 'Give_Object',
'preempted': 'Give_Object'
})
#Give the grabbed object to the person
smach.StateMachine.add('Give_Object',
move_hands_form(hand_pose_name='pre_grasp',
hand_side='right'),
transitions={
'succeeded': 'Give_Object_2',
'aborted': 'Give_Object',
'preempted': 'Give_Object'
})
smach.StateMachine.add('Give_Object_2',
move_hands_form(hand_pose_name='full_open',
hand_side='right'),
transitions={
'succeeded': 'Say_Rescue_stay',
'aborted': 'Give_Object_2',
'preempted': 'Give_Object_2'
})
smach.StateMachine.add(
'Say_Rescue_stay',
text_to_say(
'Please Stay here I am going to call for the Ambulance'),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'aborted': 'preempted'
})