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, 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, 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, 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'})