def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['object_name'],
output_keys=['standard_error','object_position','object_detected_name'])
with self:
self.userdata.standard_error = ''
smach.StateMachine.add(
'Look_down',
move_head_form(head_left_right="center", head_up_down="down"),
transitions={'succeeded': 'detect_object',
'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'detect_object',
object_detect_sm(),
transitions={'succeeded': 'process_object', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'process_object',
process_object(),
transitions={'succeeded': 'Look_normal', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'Look_normal',
move_head_form(head_left_right="center", head_up_down="normal"),
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=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):
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):
smach.StateMachine.__init__(self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
output_keys=['emergency_poi_name', 'wave_position', 'wave_yaw_degree'])
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
self.userdata.possible_pois = []
self.userdata.n_item = 0
smach.StateMachine.add(
'Prepare_Data',
Prepare_Data(),
transitions={'succeeded': 'Search_Emergency'})
smach.StateMachine.add(
'Search_Emergency',
Select_Possible_Poi(),
transitions={'succeeded':'Say_Go_to_Room','finished_searching':'aborted'})
smach.StateMachine.add(
'Say_Go_to_Room',
text_to_say("Now I am going to the next room"),
transitions={'succeeded':'Navigate_to_Room','aborted':'aborted'})
smach.StateMachine.add(
'Navigate_to_Room',
nav_to_poi(),
remapping={'nav_to_poi_name':'nav_to_poi_name_possible'},
transitions={'succeeded': 'Search_Wave', 'aborted': 'Navigate_to_Room', 'preempted': 'preempted'})
smach.StateMachine.add(
'Search_Wave',
Search_Wave_SM(head_position='down',text_for_wave_searching='Where are you? I am trying to find and help you.'),
transitions={'succeeded':'Normal_head_Out', 'preempted':'preempted',
'aborted':'aborted',
'end_searching':'Normal_head'})
smach.StateMachine.add(
'Normal_head_Out',
move_head_form(head_left_right='center', head_up_down='normal'),
transitions={'succeeded':'Prepare_Output_Search', 'preempted':'Prepare_Output_Search',
'aborted':'Prepare_Output_Search'})
smach.StateMachine.add(
'Normal_head',
move_head_form(head_left_right='center', head_up_down='normal'),
transitions={'succeeded':'Search_Emergency', 'preempted':'Search_Emergency',
'aborted':'Search_Emergency'})
smach.StateMachine.add(
'Prepare_Output_Search',
Prepare_output_search(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
def __init__(self):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
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:
self.userdata.loop_iterations = 0
self.userdata.wave_position = None
self.userdata.wave_yaw_degree = None
self.userdata.standard_error = ''
# Concurrence
sm_conc2 = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
input_keys=['tts_text', 'head_left_right', 'head_up_down'],
output_keys=[])
with sm_conc2:
# Search for face
smach.Concurrence.add('move_head_conc', move_head_form(None, None))
smach.Concurrence.add('say_conc', random_speech_say())
smach.StateMachine.add('Concurrence', sm_conc2,
transitions={'succeeded':'succeeded',
'aborted':'aborted',
'preempted':'aborted'})
smach.StateMachine.add('Wait_state',
Wait_search(),
transitions={'succeeded':'Prepare_head'})
smach.StateMachine.add('Prepare_head',
prepare_move_head('normal'),
transitions={'succeeded':'Concurrence'})
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 __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, ['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, outcomes=['succeeded','aborted', 'preempted'],
input_keys=[],
output_keys=[])
with self:
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_text = ''
self.userdata.tts_lang = 'en_US'
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 person'),
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,feedback=True):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error','in_learn_person'])
self.feedback = feedback
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': 'DEFAULT_POSITION',
'aborted': 'DEFAULT_POSITION'})
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center","up"),
transitions={'succeeded': 'WAIT_TIME','aborted':'WAIT_TIME'})
smach.StateMachine.add('WAIT_TIME',
wait_time(),
transitions={'succeeded': 'READ_TRACKER_TOPIC',
'aborted': 'READ_TRACKER_TOPIC'})
#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':'aborted',
'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.follow_pub),
transitions={'succeeded': 'NEW_PERSON',
'aborted': 'WAIT_TIME'})
smach.StateMachine.add('NEW_PERSON',
say_with_enable(text=NEW_PERSON,enable=self.feedback),
transitions={'succeeded': 'succeeded','preempted':'succeeded', 'aborted':'succeeded'})
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 main():
rospy.loginfo('Move_joint_pose_training INIT')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
#sm.userdata.move_head_pose = [-1, 0.1]
#sm.userdata.move_head_pose = [-0.5, 0.1] Mid Right
#sm.userdata.move_head_pose = [0.5, 0.1] Mid Left
#sm.userdata.move_head_pose = [1, 0.1] Total Left
#sm.userdata.move_head_pose = [0.1, 1] Head Down
#sm.userdata.move_head_pose = [0.0, 0.1] Head Normal front
smach.StateMachine.add(
'move_head',
move_head_form('mid_right', 'normal'),
transitions={'succeeded': 'succeeded','preempted':'preempted', 'aborted':'aborted'})
sm.execute()
rospy.spin()
def main():
rospy.loginfo('Move_joint_pose_training INIT')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
#sm.userdata.move_head_pose = [-1, 0.1]
#sm.userdata.move_head_pose = [-0.5, 0.1] Mid Right
#sm.userdata.move_head_pose = [0.5, 0.1] Mid Left
#sm.userdata.move_head_pose = [1, 0.1] Total Left
#sm.userdata.move_head_pose = [0.1, 1] Head Down
#sm.userdata.move_head_pose = [0.0, 0.1] Head Normal front
smach.StateMachine.add('move_head',
move_head_form('mid_right', 'normal'),
transitions={
'succeeded': 'succeeded',
'preempted': 'preempted',
'aborted': 'aborted'
})
sm.execute()
rospy.spin()
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:
self.userdata.loop_iterations = 0
self.userdata.wave_position = None
self.userdata.wave_yaw_degree = None
self.userdata.standard_error = ''
# Concurrence
sm_conc2 = smach.Concurrence(
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
input_keys=['tts_text', 'head_left_right', 'head_up_down'],
output_keys=[])
with sm_conc2:
# Search for face
smach.Concurrence.add('move_head_conc',
move_head_form(None, None))
smach.Concurrence.add('say_conc', random_speech_say())
smach.StateMachine.add('Concurrence',
sm_conc2,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'aborted'
})
smach.StateMachine.add('Wait_state',
Wait_search(),
transitions={'succeeded': 'Prepare_head'})
smach.StateMachine.add('Prepare_head',
prepare_move_head('normal'),
transitions={'succeeded': 'Concurrence'})
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,
outcomes=['succeeded', 'preempted', 'aborted'])
with self:
self.userdata.tts_wait_before_speaking=0
self.userdata.tts_text=None
self.userdata.tts_lang=None
self.userdata.nav_to_poi_name=None
self.userdata.standard_error='OK'
self.userdata.grammar_name=gramar
self.userdata.in_learn_person=1 # todo is only for try
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={'succeeded': 'CHANGE_STATE_MAP',
'aborted': 'aborted','preempted':'preempted'})
def navigation_MAP_cb(userdata,request):
update = AcknowledgmentRequest()
update.input = MAPPING_MODE
return update
def navigation_LOC_cb(userdata,request):
update = AcknowledgmentRequest()
update.input = LOCALIZATION_MODE
return update
smach.StateMachine.add('CHANGE_STATE_MAP',
ServiceState('/pal_navigation_sm',Acknowledgment,
request_cb = navigation_MAP_cb ),
transitions = {'succeeded': 'LEARN_INIT',
'aborted':'CHANGE_STATE_MAP'})
smach.StateMachine.add('LEARN_INIT',
restaurantInit(),
transitions={'succeeded': 'CONCURRENCE',
'aborted': 'LEARN_INIT','preempted':'preempted'})
sm=smach.Concurrence(outcomes=['succeeded', 'lost'],
default_outcome='succeeded',input_keys=["in_learn_person",
'grammar_name'],
child_termination_cb = child_term_cb, outcome_cb=out_cb)
with sm:
# it follow the person for long time
sm.add('FOLLOW_ME',
FollowOperator(distToHuman=0.4, feedback=False, learn_if_lost=True))
# here it have to listen and put pois in the map
sm.add('LISTEN_OPERATOR_RESTAURANT',
ListenOperator())
smach.StateMachine.add('CONCURRENCE', sm, transitions={'succeeded': 'CHANGE_STATE_STOP',
'lost':'CONCURRENCE'})
smach.StateMachine.add('CHANGE_STATE_STOP',
ServiceState('/pal_navigation_sm',Acknowledgment,
request_cb = navigation_LOC_cb ),
transitions = {'succeeded': 'DEFAULT_POSITION',
'aborted':'CHANGE_STATE_STOP'})
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center","up"),
transitions={'succeeded': 'fake_id','aborted':'fake_id'})
# it say finsih that then we can stard serving orders
smach.StateMachine.add('fake_id',
fake_id(),
transitions={'succeeded': 'FINISH',
'aborted': 'FINISH','preempted':'FINISH'})
# it say finsih that then we can stard serving orders
smach.StateMachine.add('FINISH',
text_to_say(SAY_FINISH_FOLLOWING),
transitions={'succeeded': 'succeeded',
'aborted': 'aborted','preempted':'preempted'})
def __init__(self,poi_name=None,time_out=10):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['name', 'nav_to_poi_name', 'face','face_frame'],
output_keys=['face', 'standard_error', 'face_frame'])
self.poi_name=poi_name
self.time_out=time_out
with self:
self.userdata.num_iterations = 0
self.userdata.face = None
self.userdata.wait_time = 5
self.userdata.nav_to_poi_name=None
self.userdata.face_frame=None
# We define the different points
smach.StateMachine.add(
'prepare_poi',
prepare_poi(self.poi_name),
transitions={'poi': 'go_poi', 'no_poi': 'Concurrence'})
smach.StateMachine.add(
'Say_Searching',
text_to_say('Right Now I am looking for you.',wait=True),
transitions={'succeeded': 'go_poi', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'go_poi',
nav_to_poi(poi_name),
transitions={'succeeded': 'Concurrence', 'aborted': 'Concurrence', #TODO
'preempted': 'preempted'})
# Concurrence
sm_conc = smach.Concurrence(outcomes=['succeeded', 'aborted', 'preempted', 'endTime'],
default_outcome='succeeded',
input_keys=['name', 'nav_to_poi_name', 'face', 'wait_time'],
output_keys=['face', 'standard_error', 'face_frame'],
child_termination_cb = child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# Go around the room
# smach.Concurrence.add('walk_to_poi', nav_to_poi())
#smach.Concurrence.add('turn', turn_infinit('left'))
# Move head
smach.Concurrence.add('TimeOut', TimeOut(self.time_out))
# Search for face
smach.Concurrence.add('find_faces', recognize_face_concurrent())
smach.Concurrence.add('say_search_faces', say_searching_faces())
smach.StateMachine.add('Concurrence', sm_conc,
transitions={'succeeded':'DEFAULT_POSITION',
'aborted':'Say_Finish',
'endTime': 'Say_Finish',
'preempted':'Say_Finish'})
smach.StateMachine.add(
'Say_Finish',
text_to_say('I have finish with no exit the search',wait=False),
transitions={'succeeded': 'DEFAULT_POSITION_NO', 'aborted': 'DEFAULT_POSITION_NO',
'preempted': 'preempted'})
smach.StateMachine.add('DEFAULT_POSITION_NO',
move_head_form("center","up"),
transitions={'succeeded': 'aborted','aborted':'aborted'})
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center","up"),
transitions={'succeeded': 'succeeded','aborted':'succeeded'})
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
output_keys=[
'emergency_poi_name', 'wave_position', 'wave_yaw_degree'
])
with self:
self.userdata.emergency_location = []
self.userdata.tts_lang = 'en_US'
self.userdata.tts_wait_before_speaking = 0
self.userdata.possible_pois = []
self.userdata.n_item = 0
smach.StateMachine.add(
'Prepare_Data',
Prepare_Data(),
transitions={'succeeded': 'Search_Emergency'})
smach.StateMachine.add('Search_Emergency',
Select_Possible_Poi(),
transitions={
'succeeded': 'Say_Go_to_Room',
'finished_searching': 'aborted'
})
smach.StateMachine.add(
'Say_Go_to_Room',
text_to_say("Now I am going to the next room"),
transitions={
'succeeded': 'Navigate_to_Room',
'aborted': 'aborted'
})
smach.StateMachine.add(
'Navigate_to_Room',
nav_to_poi(),
remapping={'nav_to_poi_name': 'nav_to_poi_name_possible'},
transitions={
'succeeded': 'Search_Wave',
'aborted': 'Search_Wave',
'preempted': 'preempted'
})
smach.StateMachine.add(
'Search_Wave',
Search_Wave_SM(
head_position='down',
text_for_wave_searching=
'Where are you? I am trying to find and help you.'),
transitions={
'succeeded': 'Normal_head_Out',
'preempted': 'preempted',
'aborted': 'aborted',
'end_searching': 'Normal_head'
})
smach.StateMachine.add('Normal_head_Out',
move_head_form(head_left_right='center',
head_up_down='normal'),
transitions={
'succeeded': 'Prepare_Output_Search',
'preempted': 'Prepare_Output_Search',
'aborted': 'Prepare_Output_Search'
})
smach.StateMachine.add('Normal_head',
move_head_form(head_left_right='center',
head_up_down='normal'),
transitions={
'succeeded': 'Search_Emergency',
'preempted': 'Search_Emergency',
'aborted': 'Search_Emergency'
})
smach.StateMachine.add('Prepare_Output_Search',
Prepare_output_search(),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
def __init__(self):
smach.StateMachine.__init__(
self, outcomes=['succeeded', 'preempted', 'aborted'])
with self:
self.userdata.tts_wait_before_speaking = 0
self.userdata.tts_text = None
self.userdata.tts_lang = None
self.userdata.nav_to_poi_name = None
self.userdata.standard_error = 'OK'
self.userdata.grammar_name = gramar
self.userdata.in_learn_person = 1 # todo is only for try
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={
'succeeded': 'CHANGE_STATE_MAP',
'aborted': 'aborted',
'preempted': 'preempted'
})
def navigation_MAP_cb(userdata, request):
update = AcknowledgmentRequest()
update.input = MAPPING_MODE
return update
def navigation_LOC_cb(userdata, request):
update = AcknowledgmentRequest()
update.input = LOCALIZATION_MODE
return update
smach.StateMachine.add('CHANGE_STATE_MAP',
ServiceState('/pal_navigation_sm',
Acknowledgment,
request_cb=navigation_MAP_cb),
transitions={
'succeeded': 'LEARN_INIT',
'aborted': 'CHANGE_STATE_MAP'
})
smach.StateMachine.add('LEARN_INIT',
restaurantInit(),
transitions={
'succeeded': 'CONCURRENCE',
'aborted': 'LEARN_INIT',
'preempted': 'preempted'
})
sm = smach.Concurrence(
outcomes=['succeeded', 'lost'],
default_outcome='succeeded',
input_keys=["in_learn_person", 'grammar_name'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm:
# it follow the person for long time
sm.add(
'FOLLOW_ME',
FollowOperator(distToHuman=0.4,
feedback=False,
learn_if_lost=True))
# here it have to listen and put pois in the map
sm.add('LISTEN_OPERATOR_RESTAURANT', ListenOperator())
smach.StateMachine.add('CONCURRENCE',
sm,
transitions={
'succeeded': 'CHANGE_STATE_STOP',
'lost': 'CONCURRENCE'
})
smach.StateMachine.add('CHANGE_STATE_STOP',
ServiceState('/pal_navigation_sm',
Acknowledgment,
request_cb=navigation_LOC_cb),
transitions={
'succeeded': 'DEFAULT_POSITION',
'aborted': 'CHANGE_STATE_STOP'
})
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center", "up"),
transitions={
'succeeded': 'fake_id',
'aborted': 'fake_id'
})
# it say finsih that then we can stard serving orders
smach.StateMachine.add('fake_id',
fake_id(),
transitions={
'succeeded': 'FINISH',
'aborted': 'FINISH',
'preempted': 'FINISH'
})
# it say finsih that then we can stard serving orders
smach.StateMachine.add('FINISH',
text_to_say(SAY_FINISH_FOLLOWING),
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', '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
self.userdata.standard_error = ''
smach.StateMachine.add(
'Move_head_prepare',
prepare_move_head(head_position),
transitions={'succeeded': 'Concurrence', 'aborted': 'aborted',
'preempted': 'preempted', 'end_searching':'end_searching'})
sm_conc = smach.Concurrence(outcomes = ['succeeded', 'aborted', 'preempted', 'face_detected'],
default_outcome = 'succeeded',
input_keys = ['head_left_right',
'head_up_down'],
output_keys = ['wave_position', 'wave_yaw_degree'],
child_termination_cb = child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# sm_conc.StateMachine.add(
# 'move_head',
# move_head_form(head_up_down=head_position),
# transitions={'succeeded': 'Say_Searching',
# 'preempted':'Say_Searching',
# 'aborted':'aborted'})
# sm_conc.StateMachine.add(
# 'Say_Searching',
# text_to_say(text_for_wave_searching, wait = False),
# transitions={'succeeded':'wave_recognition', 'aborted':'wave_recognition', 'preempted':'wave_recognition'})
sm_conc.add(
'move_head',
move_head_form(head_up_down=head_position))
sm_conc.add(
'Say_Searching',
text_to_say(text_for_wave_searching, wait = False))
sm_conc.add(
'wave_recognition',
WaveDetection(8.0))
smach.StateMachine.add('Concurrence',
sm_conc,
transitions={'succeeded':'Say_Found',
'aborted':'Move_head_prepare',
'preempted':'preempted',
'face_detected':'Say_Found'})
#Hard-coded maximum time in order to detect wave
# smach.StateMachine.add(
# 'wave_recognition',
# WaveDetection(4.0),
# transitions={'succeeded': 'Say_Found', 'aborted': '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, feedback=True):
smach.StateMachine.__init__(
self, ['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error', 'in_learn_person'])
self.feedback = feedback
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': 'DEFAULT_POSITION',
'aborted': 'DEFAULT_POSITION'
})
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center", "up"),
transitions={
'succeeded': 'WAIT_TIME',
'aborted': 'WAIT_TIME'
})
smach.StateMachine.add('WAIT_TIME',
wait_time(),
transitions={
'succeeded': 'READ_TRACKER_TOPIC',
'aborted': 'READ_TRACKER_TOPIC'
})
#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': 'aborted',
'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.follow_pub),
transitions={
'succeeded': 'NEW_PERSON',
'aborted': 'WAIT_TIME'
})
smach.StateMachine.add('NEW_PERSON',
say_with_enable(text=NEW_PERSON,
enable=self.feedback),
transitions={
'succeeded': 'succeeded',
'preempted': 'succeeded',
'aborted': 'succeeded'
})
def __init__(self, poi_name=None, time_out=10):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['name', 'nav_to_poi_name', 'face', 'face_frame'],
output_keys=['face', 'standard_error', 'face_frame'])
self.poi_name = poi_name
self.time_out = time_out
with self:
self.userdata.name = ''
self.userdata.num_iterations = 0
self.userdata.face = None
self.userdata.wait_time = 5
self.userdata.nav_to_poi_name = None
self.userdata.face_frame = None
# We define the different points
smach.StateMachine.add('prepare_poi',
prepare_poi(self.poi_name),
transitions={
'poi': 'go_poi',
'no_poi': 'Concurrence'
})
smach.StateMachine.add('Say_Searching',
text_to_say(
'Right Now I am looking for you.',
wait=True),
transitions={
'succeeded': 'go_poi',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add(
'go_poi',
nav_to_poi(poi_name),
transitions={
'succeeded': 'Concurrence',
'aborted': 'Concurrence', #TODO
'preempted': 'preempted'
})
# Concurrence
sm_conc = smach.Concurrence(
outcomes=['succeeded', 'aborted', 'preempted', 'endTime'],
default_outcome='succeeded',
input_keys=['name', 'nav_to_poi_name', 'face', 'wait_time'],
output_keys=['face', 'standard_error', 'face_frame'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# Go around the room
# smach.Concurrence.add('walk_to_poi', nav_to_poi())
#smach.Concurrence.add('turn', turn_infinit('left'))
# Move head
smach.Concurrence.add('TimeOut', TimeOut(self.time_out))
# Search for face
smach.Concurrence.add('find_faces',
recognize_face_concurrent())
smach.Concurrence.add('say_search_faces',
say_searching_faces())
smach.StateMachine.add('Concurrence',
sm_conc,
transitions={
'succeeded': 'DEFAULT_POSITION',
'aborted': 'Say_Finish',
'endTime': 'Say_Finish',
'preempted': 'Say_Finish'
})
smach.StateMachine.add('Say_Finish',
text_to_say(
'I have finish with no exit the search',
wait=False),
transitions={
'succeeded': 'DEFAULT_POSITION_NO',
'aborted': 'DEFAULT_POSITION_NO',
'preempted': 'preempted'
})
smach.StateMachine.add('DEFAULT_POSITION_NO',
move_head_form("center", "up"),
transitions={
'succeeded': 'aborted',
'aborted': 'aborted'
})
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center", "up"),
transitions={
'succeeded': 'succeeded',
'aborted': 'succeeded'
})
def __init__(self,
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):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error'],
input_keys=['in_learn_person'])
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
self.userdata.in_learn_person=person()
self.userdata.head_left_right=None
self.userdata.head_up_down=None
self.userdata.in_learn_person.targetId=1
print "HEY, THIS IS self.userdata.in_learn_person: " + str(self.userdata.in_learn_person)
def go_back_request(userdata,request):
start_request = NavigationGoBackRequest()
start_request.enable=True
start_request.meters=METERSBACK
return start_request
#maybe i will have to learn again
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={'succeeded': 'GO_BACK',
'aborted': 'aborted','preempted':'preempted'})
# i stop the service
def Cheack_Elevator_Stop(userdata, request):
start_request = EnableCheckElevatorRequest()
start_request.enable=False
return start_request
smach.StateMachine.add('STOP_CHECK_ELEVATOR',
ServiceState('/check_elevator/enable',
EnableCheckElevator,
request_cb = Cheack_Elevator_Stop),
transitions={'succeeded':'succeeded',
'preempted':'succeeded',
'aborted':'succeeded'})
# it will go from the lift
smach.StateMachine.add('GO_BACK',
ServiceState('/reverse',
NavigationGoBack,
request_cb = go_back_request),
transitions={'succeeded':'READ_TRACKER_TOPIC','aborted' : 'READ_TRACKER_TOPIC','preempted':'preempted'})
# i don't understant why is this goal here, i imagine for stop
smach.StateMachine.add('SEND_GOAL',
nav_to_coord_concurrent('/base_link'),
transitions={'succeeded':'READ_TRACKER_TOPIC', 'aborted':'WHERE_IS_IT','preempted':'preempted'})
smach.StateMachine.add('READ_TRACKER_TOPIC',
topic_reader(topic_name='/pipol_tracker_node/peopleSet',
topic_type=personArray,topic_time_out=60),
transitions={'succeeded':'WHERE_IS_IT',
'aborted':'READ_TRACKER_TOPIC',
'preempted':'preempted'},
remapping={'topic_output_msg': 'tracking_msg'})
# it will have to look if it can find the person, know it's only one try
smach.StateMachine.add('WHERE_IS_IT',
where_is_it(),
transitions={'i_dont_know':'SAY_COME_NEAR','ok_lets_go':'SAY_LETS_GO','preempted':'preempted'})
#this is when i don't remember the person
smach.StateMachine.add('SAY_COME_NEAR',
text_to_say(SAY_COME_NEAR),
transitions={'succeeded':'LEARN_AGAIN','aborted':'LEARN_AGAIN','preempted':'preempted'})
#hear i will learn the peron another time
smach.StateMachine.add('LEARN_AGAIN',
LearnPerson(learn_face=False),
transitions={'succeeded':'SAY_LETS_GO',
'aborted':'SAY_COME_NEAR','preempted':'preempted'})
smach.StateMachine.add('SAY_LETS_GO',
text_to_say(SAY_LETS_GO),
transitions={'succeeded':'TRACK_OPERATOR','aborted':'aborted','preempted':'preempted'})
smach.StateMachine.add('TRACK_OPERATOR',
FollowOperator(learn_if_lost=False,feedback=True),
transitions={'succeeded':'succeeded',
'lost':'LOST_CREATE_GOAL','preempted':'preempted'})
# i will prepere the navigation goal becouse i have lost the person
#the goal have to be a realy far goal
smach.StateMachine.add('LOST_CREATE_GOAL',create_nav_goal(),
transitions={'succeeded':'SEARCH_OPERATOR',
'aborted':'SEARCH_OPERATOR','preempted':'preempted'})
sm=smach.Concurrence(outcomes=['gesture_recongize', 'nav_finisheed','preempted'],
default_outcome='nav_finisheed',
input_keys=["in_learn_person",'nav_to_coord_goal'],
output_keys=['wave_position', 'wave_yaw_degree','standard_error'],
child_termination_cb = child_term_cb,
outcome_cb=out_cb_follow)
with sm:
sm.userdata.standar_error="ok"
#this goal will be a realy far goal
smach.Concurrence.add('SEND_GOAL',
nav_to_coord("/base_link"))
smach.Concurrence.add('SEARCH_OPERATOR_GESTURE',
Search_Wave_SM())
smach.StateMachine.add('SEARCH_OPERATOR', sm,
transitions={'gesture_recongize':'DEFAULT_POSITION',
'nav_finisheed':'SEARCH_OPERATOR_GESTURE_2','preempted':'preempted'})
smach.StateMachine.add('SEARCH_OPERATOR_GESTURE_2',
Search_Wave_SM(),
transitions={'succeeded':'DEFAULT_POSITION',
'aborted':'SEARCH_OPERATOR_GESTURE_2','preempted':'preempted', 'end_searching':'SEARCH_OPERATOR_GESTURE_2'})
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center","up"),
transitions={'succeeded': 'CREATE_GESTURE_NAV_GOAL','aborted':'CREATE_GESTURE_NAV_GOAL'})
smach.StateMachine.add('CREATE_GESTURE_NAV_GOAL',
create_nav_gesture_goal(),
transitions={'succeeded':'RECOGNIZE_PERSON',
'aborted':'aborted','preempted':'preempted'})
# smach.StateMachine.add('SAY_CROSSING',
# text_to_say(SAY_CROSSING),
# transitions={'succeeded':'GO_TO_GESTURE',
# 'aborted':'GO_TO_GESTURE','preempted':'preempted'})
# #if the navigation goal it's impossible it will be heare allways
# smach.StateMachine.add('GO_TO_GESTURE',
# nav_to_coord('/base_link'),
# transitions={'succeeded':'RECOGNIZE_PERSON',
# 'aborted':'RECOGNIZE_PERSON','preempted':'preempted'})
#
#when i'm whit the person i have to look if it's the person
smach.StateMachine.add('RECOGNIZE_PERSON',
LearnPerson(learn_face=False),
transitions={'succeeded':'SAY_GO_AGAIN',
'aborted':'aborted','preempted':'preempted'})
smach.StateMachine.add('SAY_GO_AGAIN',
text_to_say(SAY_GO_AGAIN),
transitions={'succeeded':'FOLLOW_AGAIN',
'aborted':'SEARCH_OPERATOR','preempted':'preempted'})
# hear i finish the state
smach.StateMachine.add('FOLLOW_AGAIN',
FollowOperator(feedback=True, learn_if_lost=True),
transitions={'succeeded':'succeeded',
'lost':'aborted','preempted':'preempted'})
def __init__(self):
smach.StateMachine.__init__(self,
['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error'],
input_keys=['in_learn_person'])
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
self.userdata.in_learn_person = person()
self.userdata.head_left_right = None
self.userdata.head_up_down = None
self.userdata.in_learn_person.targetId = 1
print "HEY, THIS IS self.userdata.in_learn_person: " + str(
self.userdata.in_learn_person)
def go_back_request(userdata, request):
start_request = NavigationGoBackRequest()
start_request.enable = True
start_request.meters = METERSBACK
return start_request
#maybe i will have to learn again
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={
'succeeded': 'GO_BACK',
'aborted': 'aborted',
'preempted': 'preempted'
})
# i stop the service
def Cheack_Elevator_Stop(userdata, request):
start_request = EnableCheckElevatorRequest()
start_request.enable = False
return start_request
smach.StateMachine.add('STOP_CHECK_ELEVATOR',
ServiceState(
'/check_elevator/enable',
EnableCheckElevator,
request_cb=Cheack_Elevator_Stop),
transitions={
'succeeded': 'succeeded',
'preempted': 'succeeded',
'aborted': 'succeeded'
})
# it will go from the lift
smach.StateMachine.add('GO_BACK',
ServiceState('/reverse',
NavigationGoBack,
request_cb=go_back_request),
transitions={
'succeeded': 'READ_TRACKER_TOPIC',
'aborted': 'READ_TRACKER_TOPIC',
'preempted': 'preempted'
})
# i don't understant why is this goal here, i imagine for stop
smach.StateMachine.add('SEND_GOAL',
nav_to_coord_concurrent('/base_link'),
transitions={
'succeeded': 'READ_TRACKER_TOPIC',
'aborted': 'WHERE_IS_IT',
'preempted': 'preempted'
})
smach.StateMachine.add(
'READ_TRACKER_TOPIC',
topic_reader(topic_name='/pipol_tracker_node/peopleSet',
topic_type=personArray,
topic_time_out=60),
transitions={
'succeeded': 'WHERE_IS_IT',
'aborted': 'READ_TRACKER_TOPIC',
'preempted': 'preempted'
},
remapping={'topic_output_msg': 'tracking_msg'})
# it will have to look if it can find the person, know it's only one try
smach.StateMachine.add('WHERE_IS_IT',
where_is_it(),
transitions={
'i_dont_know': 'SAY_COME_NEAR',
'ok_lets_go': 'SAY_LETS_GO',
'preempted': 'preempted'
})
#this is when i don't remember the person
smach.StateMachine.add('SAY_COME_NEAR',
text_to_say(SAY_COME_NEAR),
transitions={
'succeeded': 'LEARN_AGAIN',
'aborted': 'LEARN_AGAIN',
'preempted': 'preempted'
})
#hear i will learn the peron another time
smach.StateMachine.add('LEARN_AGAIN',
LearnPerson(learn_face=False),
transitions={
'succeeded': 'SAY_LETS_GO',
'aborted': 'SAY_COME_NEAR',
'preempted': 'preempted'
})
smach.StateMachine.add('SAY_LETS_GO',
text_to_say(SAY_LETS_GO),
transitions={
'succeeded': 'TRACK_OPERATOR',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('TRACK_OPERATOR',
FollowOperator(learn_if_lost=False,
feedback=True),
transitions={
'succeeded': 'succeeded',
'lost': 'LOST_CREATE_GOAL',
'preempted': 'preempted'
})
# i will prepere the navigation goal becouse i have lost the person
#the goal have to be a realy far goal
smach.StateMachine.add('LOST_CREATE_GOAL',
create_nav_goal(),
transitions={
'succeeded': 'SEARCH_OPERATOR',
'aborted': 'SEARCH_OPERATOR',
'preempted': 'preempted'
})
sm = smach.Concurrence(
outcomes=['gesture_recongize', 'nav_finisheed', 'preempted'],
default_outcome='nav_finisheed',
input_keys=["in_learn_person", 'nav_to_coord_goal'],
output_keys=[
'wave_position', 'wave_yaw_degree', 'standard_error'
],
child_termination_cb=child_term_cb,
outcome_cb=out_cb_follow)
with sm:
sm.userdata.standar_error = "ok"
#this goal will be a realy far goal
smach.Concurrence.add('SEND_GOAL', nav_to_coord("/base_link"))
smach.Concurrence.add('SEARCH_OPERATOR_GESTURE',
Search_Wave_SM())
smach.StateMachine.add('SEARCH_OPERATOR',
sm,
transitions={
'gesture_recongize': 'DEFAULT_POSITION',
'nav_finisheed':
'SEARCH_OPERATOR_GESTURE_2',
'preempted': 'preempted'
})
smach.StateMachine.add('SEARCH_OPERATOR_GESTURE_2',
Search_Wave_SM(),
transitions={
'succeeded': 'DEFAULT_POSITION',
'aborted': 'SEARCH_OPERATOR_GESTURE_2',
'preempted': 'preempted',
'end_searching':
'SEARCH_OPERATOR_GESTURE_2'
})
smach.StateMachine.add('DEFAULT_POSITION',
move_head_form("center", "up"),
transitions={
'succeeded': 'CREATE_GESTURE_NAV_GOAL',
'aborted': 'CREATE_GESTURE_NAV_GOAL'
})
smach.StateMachine.add('CREATE_GESTURE_NAV_GOAL',
create_nav_gesture_goal(),
transitions={
'succeeded': 'RECOGNIZE_PERSON',
'aborted': 'aborted',
'preempted': 'preempted'
})
# smach.StateMachine.add('SAY_CROSSING',
# text_to_say(SAY_CROSSING),
# transitions={'succeeded':'GO_TO_GESTURE',
# 'aborted':'GO_TO_GESTURE','preempted':'preempted'})
# #if the navigation goal it's impossible it will be heare allways
# smach.StateMachine.add('GO_TO_GESTURE',
# nav_to_coord('/base_link'),
# transitions={'succeeded':'RECOGNIZE_PERSON',
# 'aborted':'RECOGNIZE_PERSON','preempted':'preempted'})
#
#when i'm whit the person i have to look if it's the person
smach.StateMachine.add('RECOGNIZE_PERSON',
LearnPerson(learn_face=False),
transitions={
'succeeded': 'SAY_GO_AGAIN',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('SAY_GO_AGAIN',
text_to_say(SAY_GO_AGAIN),
transitions={
'succeeded': 'FOLLOW_AGAIN',
'aborted': 'SEARCH_OPERATOR',
'preempted': 'preempted'
})
# hear i finish the state
smach.StateMachine.add('FOLLOW_AGAIN',
FollowOperator(feedback=True,
learn_if_lost=True),
transitions={
'succeeded': 'succeeded',
'lost': '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', '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
self.userdata.standard_error = ''
smach.StateMachine.add('Move_head_prepare',
prepare_move_head(head_position),
transitions={
'succeeded': 'Concurrence',
'aborted': 'aborted',
'preempted': 'preempted',
'end_searching': 'end_searching'
})
sm_conc = smach.Concurrence(
outcomes=[
'succeeded', 'aborted', 'preempted', 'face_detected'
],
default_outcome='succeeded',
input_keys=['head_left_right', 'head_up_down'],
output_keys=['wave_position', 'wave_yaw_degree'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with sm_conc:
# sm_conc.StateMachine.add(
# 'move_head',
# move_head_form(head_up_down=head_position),
# transitions={'succeeded': 'Say_Searching',
# 'preempted':'Say_Searching',
# 'aborted':'aborted'})
# sm_conc.StateMachine.add(
# 'Say_Searching',
# text_to_say(text_for_wave_searching, wait = False),
# transitions={'succeeded':'wave_recognition', 'aborted':'wave_recognition', 'preempted':'wave_recognition'})
sm_conc.add('move_head',
move_head_form(head_up_down=head_position))
sm_conc.add('Say_Searching',
text_to_say(text_for_wave_searching, wait=False))
sm_conc.add('wave_recognition', WaveDetection(8.0))
smach.StateMachine.add('Concurrence',
sm_conc,
transitions={
'succeeded': 'Say_Found',
'aborted': 'Move_head_prepare',
'preempted': 'preempted',
'face_detected': 'Say_Found'
})
#Hard-coded maximum time in order to detect wave
# smach.StateMachine.add(
# 'wave_recognition',
# WaveDetection(4.0),
# transitions={'succeeded': 'Say_Found', 'aborted': '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'
})
