def __init__(self, time_for_wave=5.0):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
output_keys=['wave_position', 'wave_yaw_degree', 'standard_error'])
with self:
self.userdata.standard_error = ''
smach.StateMachine.add('Gesture_Topic_Reader',
topic_reader(topic_name=GESTURE_TOPIC,
topic_type=Gesture,
topic_time_out=time_for_wave,
blocked=False),
transitions={
'succeeded': 'TransformGesture',
'preempted': 'preempted',
'aborted': 'aborted'
})
#topic_reader(topic_name=GESTURE_TOPIC, topic_type=Gesture, topic_time_out=60.0, blocked=False),
# transitions={'succeeded':'TransformGesture', 'preempted':'Gesture_Topic_Reader', 'aborted':'Gesture_Topic_Reader'})
smach.StateMachine.add('TransformGesture',
TransformGesture(),
transitions={
'succeeded': 'succeeded',
'preempted': 'preempted',
'aborted': 'aborted'
})
def __init__(self,minConfidence=90.0):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=[],
output_keys=['standard_error','faces'])
with self:
self.userdata.faces = ""
# call request for Recognizer
@smach.cb_interface(input_keys=[])
def face_start_detect_request_cb(userdata, request):
start_request = RecognizerRequest()
start_request.enabled=True
start_request.minConfidence=minConfidence
return start_request
#call request of start recognizer
smach.StateMachine.add('Start_recognizer',
ServiceState('/pal_face/recognizer',
Recognizer,
request_cb = face_start_detect_request_cb,
input_keys = []),
transitions={'succeeded':'Read_Topic','aborted' : 'aborted','preempted':'preempted'})
smach.StateMachine.add(
'Read_Topic',
topic_reader('/pal_face/faces', FaceDetections, 60),
remapping={'topic_output_msg': 'faces'},
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
def __init__(self,learn_face=False):
smach.StateMachine.__init__(self, ['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error','in_learn_person'])
self.learn_face = learn_face
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': 'READ_TRACKER_TOPIC',
'aborted': 'SAY_LIFTIME'})
smach.StateMachine.add('SAY_LIFTIME',
text_to_say(SAY_GO_MIDLE),
transitions={'succeeded': 'CONTROL_TIME',
'aborted': 'SAY_LIFTIME'})
smach.StateMachine.add('CONTROL_TIME',
control_time(),
transitions={'succeeded': 'READ_TRACKER_TOPIC',
'lif_time': 'SAY_LIFTIME'})
#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':'READ_TRACKER_TOPIC',
'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.learn_face, self.follow_pub),
transitions={'succeeded': 'STOP_LEARNING',
'aborted': 'CONTROL_TIME','learn_face':'FACE_SAY'})
smach.StateMachine.add('FACE_SAY',
text_to_say(SAY_LEARN),
transitions={'succeeded': 'LEARN_FACE','aborted':'LEARN_FACE'})
# it learns the person that we have to follow
smach.StateMachine.add('LEARN_FACE',
new_database_and_learn(learning_time=2, minConfidence=80,
name_face="following", name_database="follow"),
transitions={'succeeded': 'STOP_LEARNING',
'aborted': 'CONTROL_TIME'})
smach.StateMachine.add('STOP_LEARNING',
text_to_say(SAY_FINISH),
transitions={'succeeded': 'succeeded','aborted':'STOP_LEARNING'})
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'
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={
'succeeded': 'SAY_LIFTIME',
'aborted': 'SAY_LIFTIME'
})
smach.StateMachine.add('SAY_LIFTIME',
text_to_say(SAY_GO_MIDLE),
transitions={
'succeeded': 'CONTROL_TIME',
'aborted': 'SAY_LIFTIME'
})
smach.StateMachine.add('CONTROL_TIME',
control_time(),
transitions={
'succeeded': 'READ_TRACKER_TOPIC',
'lif_time': 'INIT_VAR'
})
smach.StateMachine.add(
'READ_TRACKER_TOPIC',
topic_reader(topic_name='/people_tracker_node/peopleSet',
topic_type=personArray,
topic_time_out=60),
transitions={
'succeeded': 'SELECT_ID',
'aborted': 'READ_TRACKER_TOPIC',
'preempted': 'preempted'
},
remapping={'topic_output_msg': 'tracking_msg'})
# it learns the person that we have to follow
smach.StateMachine.add('SELECT_ID',
select_ID(),
transitions={
'succeeded': 'STOP_LEARNING',
'aborted': 'CONTROL_TIME'
})
smach.StateMachine.add('STOP_LEARNING',
text_to_say("OK i have finsished"),
transitions={
'succeeded': 'succeeded',
'aborted': 'STOP_LEARNING'
})
def __init__(self, time_for_wave=20.0):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[],
output_keys=['wave_position', 'wave_yaw_degree','standard_error'])
with self:
smach.StateMachine.add(
'Gesture_Topic_Reader',
topic_reader(topic_name=GESTURE_TOPIC, topic_type=Gesture, topic_time_out=time_for_wave, blocked=False),
transitions={'succeeded':'TransformGesture', 'preempted':'preempted', 'aborted':'aborted'})
#topic_reader(topic_name=GESTURE_TOPIC, topic_type=Gesture, topic_time_out=60.0, blocked=False),
# transitions={'succeeded':'TransformGesture', 'preempted':'Gesture_Topic_Reader', 'aborted':'Gesture_Topic_Reader'})
smach.StateMachine.add(
'TransformGesture',
TransformGesture(),
transitions={'succeeded':'succeeded', 'preempted':'preempted', '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):
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'
smach.StateMachine.add('INIT_VAR',
init_var(),
transitions={'succeeded': 'SAY_LIFTIME',
'aborted': 'SAY_LIFTIME'})
smach.StateMachine.add('SAY_LIFTIME',
text_to_say(SAY_GO_MIDLE),
transitions={'succeeded': 'CONTROL_TIME',
'aborted': 'SAY_LIFTIME'})
smach.StateMachine.add('CONTROL_TIME',
control_time(),
transitions={'succeeded': 'READ_TRACKER_TOPIC',
'lif_time': 'INIT_VAR'})
smach.StateMachine.add('READ_TRACKER_TOPIC',
topic_reader(topic_name='/people_tracker_node/peopleSet',
topic_type=personArray,topic_time_out=60),
transitions={'succeeded':'SELECT_ID',
'aborted':'READ_TRACKER_TOPIC',
'preempted':'preempted'},
remapping={'topic_output_msg': 'tracking_msg'})
# it learns the person that we have to follow
smach.StateMachine.add('SELECT_ID',
select_ID(),
transitions={'succeeded': 'STOP_LEARNING',
'aborted': 'CONTROL_TIME'})
smach.StateMachine.add('STOP_LEARNING',
text_to_say("OK i have finsished"),
transitions={'succeeded': 'succeeded','aborted':'STOP_LEARNING'})
def __init__(self, minConfidence=90.0):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=[],
output_keys=['standard_error', 'faces'])
with self:
self.userdata.faces = ""
# call request for Recognizer
@smach.cb_interface(input_keys=[])
def face_start_detect_request_cb(userdata, request):
start_request = RecognizerRequest()
start_request.enabled = True
start_request.minConfidence = minConfidence
return start_request
#call request of start recognizer
smach.StateMachine.add('Start_recognizer',
ServiceState(
'/pal_face/recognizer',
Recognizer,
request_cb=face_start_detect_request_cb,
input_keys=[]),
transitions={
'succeeded': 'Read_Topic',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('Read_Topic',
topic_reader('/pal_face/faces',
FaceDetections, 60),
remapping={'topic_output_msg': 'faces'},
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, 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,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.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, learn_face=False):
smach.StateMachine.__init__(
self, ['succeeded', 'preempted', 'aborted'],
output_keys=['standard_error', 'in_learn_person'])
self.learn_face = learn_face
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': 'READ_TRACKER_TOPIC',
'aborted': 'SAY_LIFTIME'
})
smach.StateMachine.add('SAY_LIFTIME',
text_to_say(SAY_GO_MIDLE),
transitions={
'succeeded': 'CONTROL_TIME',
'aborted': 'SAY_LIFTIME'
})
smach.StateMachine.add('CONTROL_TIME',
control_time(),
transitions={
'succeeded': 'READ_TRACKER_TOPIC',
'lif_time': 'SAY_LIFTIME'
})
#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': 'READ_TRACKER_TOPIC',
'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.learn_face, self.follow_pub),
transitions={
'succeeded': 'STOP_LEARNING',
'aborted': 'CONTROL_TIME',
'learn_face': 'FACE_SAY'
})
smach.StateMachine.add('FACE_SAY',
text_to_say(SAY_LEARN),
transitions={
'succeeded': 'LEARN_FACE',
'aborted': 'LEARN_FACE'
})
# it learns the person that we have to follow
smach.StateMachine.add('LEARN_FACE',
new_database_and_learn(
learning_time=2,
minConfidence=80,
name_face="following",
name_database="follow"),
transitions={
'succeeded': 'STOP_LEARNING',
'aborted': 'CONTROL_TIME'
})
smach.StateMachine.add('STOP_LEARNING',
text_to_say(SAY_FINISH),
transitions={
'succeeded': 'succeeded',
'aborted': 'STOP_LEARNING'
})
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.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'})
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'})