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'})
예제 #2
0
    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'
                                   })
예제 #3
0
    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'
                                   })
예제 #4
0
    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'})