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, distToHuman=0.4,time_occluded=30,feedback=True,learn_if_lost=True): smach.StateMachine.__init__( self, outcomes=['succeeded', 'lost','preempted'], input_keys=['in_learn_person']) self.learn_if_lost=learn_if_lost self.feedback=feedback with self: self.userdata.old_status=0 self.userdata.feadback=0 # that means that we don't have feadback self.userdata.standard_error='OK' #self.userdata.in_learn_person=1 self.userdata.word_to_listen=None self.userdata.tts_wait_before_speaking=0 self.userdata.tts_text=None self.userdata.tts_lang='en_US' smach.StateMachine.add('INIT_VAR', init_var(), transitions={'succeeded': "READ_TRACKER_TOPIC", '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':'FILTER_AND_PROCESS', 'aborted':'READ_TRACKER_TOPIC', 'preempted':'preempted'}, remapping={'topic_output_msg': 'tracking_msg'}) # in this state i filter the message and give only a person_msg # I_KNOW is that i have find the targed_id in personArray # not_found is that i don't smach.StateMachine.add('FILTER_AND_PROCESS', filter_and_process(), transitions={'find_it': 'CALCULATE_GOAL', 'occluded':'CALCULATE_GOAL', 'not_find': 'I_DONT_KNOW', 'preempted':'preempted'}) # this state now it's dummy, maybe we will like to do something before throw in the towel smach.StateMachine.add('I_DONT_KNOW', text_to_say(LOST_SENTENCE), transitions={'succeeded': 'lost','preempted':'preempted', 'aborted':'lost'}) smach.StateMachine.add('LEARN_RANDOM', LearnPersonRandom(), transitions={'succeeded':'READ_TRACKER_TOPIC','preempted':'preempted','aborted':'lost'}) smach.StateMachine.add('CALCULATE_GOAL', calculate_goal(distToHuman), transitions={'succeeded': 'SEND_GOAL', 'aborted': 'READ_TRACKER_TOPIC','preempted':'preempted'}) smach.StateMachine.add('SEND_GOAL', nav_to_coord_concurrent('/base_link'), transitions={'succeeded':'FREQ_SENDING', 'aborted':'FREQ_SENDING','preempted':'preempted'}) smach.StateMachine.add('FREQ_SENDING', freq_goal(), transitions={'succeeded':'READ_TRACKER_TOPIC','preempted':'preempted'}) # todo before transitions={'succeeded':'DEBUG','preempted':'preempted'})
def __init__(self, distToHuman=0.9, time_occluded=30): smach.StateMachine.__init__(self, outcomes=["succeeded", "lost", "preempted"], 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.nav_to_poi_name = None self.userdata.standard_error = "OK" self.userdata.grammar_name = "restaurant.gram" self.userdata.standard_error = "OK" smach.StateMachine.add( "INIT_VAR", init_var(), transitions={"succeeded": "READ_TRACKER_TOPIC", "preempted": "preempted"} ) smach.StateMachine.add( "READ_TRACKER_TOPIC", topic_reader(topic_name="/people_tracker/person", topic_type=tracker_people, topic_time_out=60), transitions={ "succeeded": "FILTER_AND_PROCESS", "aborted": "READ_TRACKER_TOPIC", "preempted": "preempted", }, remapping={"topic_output_msg": "tracking_msg"}, ) smach.StateMachine.add( "FILTER_AND_PROCESS", filter_and_process(), transitions={ "find_it": "I_KNOW", "not_found": "I_DONT_KNOW", "no_pausible": "READ_TRACKER_TOPIC", "preempted": "preempted", }, ) smach.StateMachine.add( "I_KNOW", reset_occluded_timer(), transitions={"succeeded": "CALCULATE_GOAL", "preempted": "preempted"} ) # hear we will comprobate the time, if its greater than LOST_TIME it will return Lost smach.StateMachine.add( "I_DONT_KNOW", no_follow(time_occluded), transitions={"lost": "lost", "occluded": "READ_TRACKER_TOPIC", "preempted": "preempted"}, ) # /move_base_simple/goal smach.StateMachine.add( "CALCULATE_GOAL", calculate_goal(distToHuman), transitions={"succeeded": "SEND_GOAL", "aborted": "READ_TRACKER_TOPIC", "preempted": "preempted"}, ) smach.StateMachine.add( "SEND_GOAL", nav_to_coord_concurrent("/base_link"), transitions={"succeeded": "DEBUG", "aborted": "DEBUG", "preempted": "preempted"}, ) # it have to desaper smach.StateMachine.add( "DEBUG", debug(), transitions={"succeeded": "READ_TRACKER_TOPIC", "preempted": "preempted"} )
def __init__(self, distToHuman=0.4, feedback=True, learn_if_lost=True): smach.StateMachine.__init__( self, outcomes=['succeeded', 'lost', 'preempted'], input_keys=['in_learn_person']) self.feedback = feedback self.learn_if_lost = learn_if_lost with self: self.userdata.old_status = 0 self.userdata.feadback = 0 # that means that we don't have feadback self.userdata.standard_error = 'OK' #self.userdata.in_learn_person=1 self.userdata.word_to_listen = None self.userdata.tts_wait_before_speaking = 0 self.userdata.tts_text = None self.userdata.tts_lang = 'en_US' smach.StateMachine.add('INIT_VAR', init_var(), transitions={ 'succeeded': "READ_TRACKER_TOPIC", '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': 'FILTER_AND_PROCESS', 'aborted': 'READ_TRACKER_TOPIC', 'preempted': 'preempted' }, remapping={'topic_output_msg': 'tracking_msg'}) # in this state i filter the message and give only a person_msg # I_KNOW is that i have find the targed_id in personArray # not_found is that i don't smach.StateMachine.add('FILTER_AND_PROCESS', filter_and_process(), transitions={ 'find_it': 'CALCULATE_GOAL', 'occluded': 'OCCLUDED_PERSON', 'not_find': 'I_DONT_KNOW', 'preempted': 'preempted' }) # this state now it's dummy, maybe we will like to do something if it's to long time smach.StateMachine.add( 'OCCLUDED_PERSON', occluded_person(), transitions={ 'occluded': 'CALCULATE_GOAL', # TODO avans i havia 'occluded':'DEBUG' 'succeeded': 'CALCULATE_GOAL', 'preempted': 'preempted' }) # this state now it's dummy, maybe we will like to do something before throw in the towel smach.StateMachine.add('I_DONT_KNOW', say_with_enable(text=LOST_SENTENCE, enable=self.feedback, wait=False), transitions={ 'succeeded': 'Check_Learn_random', 'preempted': 'preempted', 'aborted': 'Check_Learn_random' }) # this state now it's dummy, maybe we will like to do something before throw in the towel smach.StateMachine.add('Check_Learn_random', Check_variable(self.learn_if_lost), transitions={ 'succeeded': 'LEARN_RANDOM', 'preempted': 'preempted', 'aborted': 'lost' }) smach.StateMachine.add('LEARN_RANDOM', LearnPersonRandom(self.feedback), transitions={ 'succeeded': 'READ_TRACKER_TOPIC', 'preempted': 'preempted', 'aborted': 'lost' }) smach.StateMachine.add('CALCULATE_GOAL', calculate_goal(distToHuman), transitions={ 'succeeded': 'SEND_GOAL', 'aborted': 'READ_TRACKER_TOPIC', 'preempted': 'preempted' }) smach.StateMachine.add('SEND_GOAL', nav_to_coord_concurrent('/base_link'), transitions={ 'succeeded': 'FREQ_SENDING', 'aborted': 'DEBUG', 'preempted': 'preempted' }) # it look if we want a feedback or not smach.StateMachine.add( 'FREQ_SENDING', freq_goal(self.feedback), transitions={ 'no_feedback': 'READ_TRACKER_TOPIC', 'feedback': 'DEBUG' } ) # todo before transitions={'succeeded':'DEBUG','preempted':'preempted'}) # it have to desaper smach.StateMachine.add('DEBUG', debug(), transitions={ 'succeeded': 'CREATE_FEADBACK', 'preempted': 'preempted' }) smach.StateMachine.add('CREATE_FEADBACK', feadback(), transitions={ 'liftime': 'SAY_FEADBACK', 'no_liftime': 'READ_TRACKER_TOPIC' }) smach.StateMachine.add('SAY_FEADBACK', text_to_say(wait=False), transitions={ 'succeeded': 'READ_TRACKER_TOPIC', 'aborted': 'SAY_FEADBACK', '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 = 1 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' }) 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': 'SEND_GOAL', 'aborted': 'SEND_GOAL', 'preempted': 'preempted' }) smach.StateMachine.add('SEND_GOAL', nav_to_coord_concurrent('/base_link'), transitions={ 'succeeded': 'WHERE_IS_IT', 'aborted': 'WHERE_IS_IT', 'preempted': 'preempted' }) # it will go out from the lift 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', learn_again(), 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(), 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"], child_termination_cb=child_term_cb, outcome_cb=out_cb_follow, output_keys=[]) with sm: sm.userdata.standar_error = "ok" #this goal will be a realy far goal smach.Concurrence.add('SEND_GOAL', nav_to_coord()) smach.Concurrence.add('SEARCH_OPERATOR_GESTURE', gesture_recogniton()) smach.StateMachine.add('SEARCH_OPERATOR', sm, transitions={ 'gesture_recongize': 'CREATE_GESTURE_NAV_GOAL', 'nav_finisheed': 'SEARCH_OPERATOR', 'preempted': 'preempted' }) smach.StateMachine.add('CREATE_GESTURE_NAV_GOAL', create_nav_gesture_goal(), transitions={ 'succeeded': 'GO_TO_GESTURE', 'aborted': 'aborted', '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', recognize_person(), 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(), transitions={ 'succeeded': 'succeeded', 'lost': 'aborted', 'preempted': 'preempted' })
def __init__(self, distToHuman=0.9, time_occluded=30): smach.StateMachine.__init__( self, outcomes=['succeeded', 'lost', 'preempted'], 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.nav_to_poi_name = None self.userdata.standard_error = 'OK' self.userdata.grammar_name = "restaurant.gram" self.userdata.standard_error = 'OK' smach.StateMachine.add('INIT_VAR', init_var(), transitions={ 'succeeded': "READ_TRACKER_TOPIC", 'preempted': 'preempted' }) smach.StateMachine.add( 'READ_TRACKER_TOPIC', topic_reader(topic_name='/people_tracker/person', topic_type=tracker_people, topic_time_out=60), transitions={ 'succeeded': 'FILTER_AND_PROCESS', 'aborted': 'READ_TRACKER_TOPIC', 'preempted': 'preempted' }, remapping={'topic_output_msg': 'tracking_msg'}) smach.StateMachine.add('FILTER_AND_PROCESS', filter_and_process(), transitions={ 'find_it': 'I_KNOW', 'not_found': 'I_DONT_KNOW', 'no_pausible': 'READ_TRACKER_TOPIC', 'preempted': 'preempted' }) smach.StateMachine.add('I_KNOW', reset_occluded_timer(), transitions={ 'succeeded': 'CALCULATE_GOAL', 'preempted': 'preempted' }) # hear we will comprobate the time, if its greater than LOST_TIME it will return Lost smach.StateMachine.add('I_DONT_KNOW', no_follow(time_occluded), transitions={ 'lost': 'lost', 'occluded': 'READ_TRACKER_TOPIC', 'preempted': 'preempted' }) # /move_base_simple/goal smach.StateMachine.add('CALCULATE_GOAL', calculate_goal(distToHuman), transitions={ 'succeeded': 'SEND_GOAL', 'aborted': 'READ_TRACKER_TOPIC', 'preempted': 'preempted' }) smach.StateMachine.add('SEND_GOAL', nav_to_coord_concurrent('/base_link'), transitions={ 'succeeded': 'DEBUG', 'aborted': 'DEBUG', 'preempted': 'preempted' }) # it have to desaper smach.StateMachine.add('DEBUG', debug(), transitions={ 'succeeded': 'READ_TRACKER_TOPIC', '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, distToHuman=0.9,time_occluded=30): smach.StateMachine.__init__( self, outcomes=['succeeded', 'lost','preempted'], input_keys=[]) with self: self.userdata.old_status=0 self.userdata.feadback=0 # that means that we don't have feadback self.userdata.standard_error='OK' self.userdata.in_learn_person=1 self.userdata.word_to_listen=None self.userdata.tts_wait_before_speaking=0 self.userdata.tts_text=None self.userdata.tts_lang='en_US' smach.StateMachine.add('INIT_VAR', init_var(), transitions={'succeeded': "READ_TRACKER_TOPIC", 'preempted':'preempted'}) smach.StateMachine.add('READ_TRACKER_TOPIC', topic_reader(topic_name='/people_tracker_node/peopleSet', topic_type=personArray,topic_time_out=60), transitions={'succeeded':'FILTER_AND_PROCESS', 'aborted':'READ_TRACKER_TOPIC', 'preempted':'preempted'}, remapping={'topic_output_msg': 'tracking_msg'}) # in this state i filter the message and give only a person_msg # I_KNOW is that i have find the targed_id in personArray # not_found is that i don't smach.StateMachine.add('FILTER_AND_PROCESS', filter_and_process(), transitions={'find_it': 'CALCULATE_GOAL', 'occluded':'OCCLUDED_PERSON', 'not_find': 'I_DONT_KNOW', 'preempted':'preempted'}) # this state now it's dummy, maybe we will like to do something if it's to long time smach.StateMachine.add('OCCLUDED_PERSON', occluded_person(), transitions={'occluded': 'DEBUG', 'succeeded':'CALCULATE_GOAL', 'preempted':'preempted'}) # this state now it's dummy, maybe we will like to do something before throw in the towel smach.StateMachine.add('I_DONT_KNOW', no_follow(), transitions={'lost': 'lost','preempted':'preempted'}) smach.StateMachine.add('CALCULATE_GOAL', calculate_goal(distToHuman), transitions={'succeeded': 'SEND_GOAL', 'aborted': 'READ_TRACKER_TOPIC','preempted':'preempted'}) smach.StateMachine.add('SEND_GOAL', nav_to_coord_concurrent('/base_link'), transitions={'succeeded':'FREQ_SENDING', 'aborted':'DEBUG','preempted':'preempted'}) smach.StateMachine.add('FREQ_SENDING', freq_goal(), transitions={'succeeded':'DEBUG','preempted':'preempted'}) smach.StateMachine.add('CREATE_FEADBACK', feadback(), transitions={'liftime':'SAY_FEADBACK','no_liftime':'READ_TRACKER_TOPIC'}) smach.StateMachine.add('SAY_FEADBACK', text_to_say(), transitions={'succeeded':'READ_TRACKER_TOPIC','aborted':'SAY_FEADBACK', 'preempted':'preempted'}) # it have to desaper smach.StateMachine.add('DEBUG', debug(), transitions={'succeeded': 'CREATE_FEADBACK','preempted':'preempted'})