def execute(self, userdata):
userdata.old_status=userdata.old_status
userdata.feadback=userdata.feadback
rospy.logwarn("-------olf status : "+str(userdata.old_status)+ "mew satusss : "+str(userdata.feadback))
msg=person()
# it means that is not the first time
if userdata.old_status == userdata.feadback :
if (userdata.old_status==OCLUDED) and (rospy.get_rostime().secs-self.time_voice_init.secs)>TIME_SPEACK_OCLUDED :
self.time_voice_init=rospy.get_rostime()
userdata.tts_text=TIME_OCLUDED_SAY
return 'liftime'
if (userdata.old_status==OKI) and (rospy.get_rostime().secs-self.time_voice_init.secs)>TIME_SPEACK_OK :
self.time_voice_init=rospy.get_rostime()
userdata.tts_text=TIME_OK_SAY
return 'liftime'
if (userdata.old_status==NEAR) and (rospy.get_rostime().secs-self.time_voice_init.secs)>TIME_SPEACK_NEAR :
self.time_voice_init=rospy.get_rostime()
userdata.tts_text=TIME_NEAR_SAY
return 'liftime'
else :
self.time_voice_init=rospy.get_rostime()
userdata.old_status=userdata.feadback
userdata.old_status = userdata.feadback
return 'no_liftime'
return 'no_liftime'
def execute(self, userdata):
userdata.old_status = userdata.old_status
userdata.feadback = userdata.feadback
rospy.logwarn("-------olf status : " + str(userdata.old_status) +
"mew satusss : " + str(userdata.feadback))
msg = person()
# it means that is not the first time
if userdata.old_status == userdata.feadback:
if (userdata.old_status == OCLUDED
) and (rospy.get_rostime().secs -
self.time_voice_init.secs) > TIME_SPEACK_OCLUDED:
self.time_voice_init = rospy.get_rostime()
userdata.tts_text = TIME_OCLUDED_SAY
return 'liftime'
if (userdata.old_status
== OKI) and (rospy.get_rostime().secs -
self.time_voice_init.secs) > TIME_SPEACK_OK:
self.time_voice_init = rospy.get_rostime()
userdata.tts_text = TIME_OK_SAY
return 'liftime'
if (userdata.old_status == NEAR
) and (rospy.get_rostime().secs -
self.time_voice_init.secs) > TIME_SPEACK_NEAR:
self.time_voice_init = rospy.get_rostime()
userdata.tts_text = TIME_NEAR_SAY
return 'liftime'
else:
self.time_voice_init = rospy.get_rostime()
userdata.old_status = userdata.feadback
userdata.old_status = userdata.feadback
return 'no_liftime'
return 'no_liftime'
def execute(self, userdata):
userdata.in_learn_person = None
person_detect = userdata.tracking_msg
per_aux = person()
#per_follow=[]
minx = X_CALIBRATION
found = False
for person_aux in person_detect.peopleSet:
if (-Y_CALIBRARTION < person_aux.y < Y_CALIBRARTION) and (
person_aux.targetStatus & person.VISUALLY_CONFIRMED):
if person_aux.x < minx:
minx = person_aux.x
userdata.in_learn_person = person_aux
found = True
if found:
self.pub.publish(userdata.in_learn_person.targetId)
rospy.loginfo(OKGREEN + "i have learned the person whit ID : " +
str(userdata.in_learn_person) + ENDC)
if (self.learn_face):
return 'learn_face'
else:
return 'succeeded'
userdata.in_learn_person = None
return 'aborted'
def execute(self, userdata):
userdata.in_learn_person=None
person_detect=userdata.tracking_msg
per_aux=person()
#per_follow=[]
minx=X_CALIBRATION
found =False
for person_aux in person_detect.peopleSet :
if (-Y_CALIBRARTION<person_aux.y<Y_CALIBRARTION) and ((person_aux.targetStatus & person.VISUALLY_CONFIRMED) or (person_aux.targetStatus & person.OCCLUDDED)):
if person_aux.x<minx :
minx=person_aux.x
userdata.in_learn_person=person_aux
found=True
if found :
self.pub.publish(userdata.in_learn_person.targetId)
rospy.loginfo(OKGREEN+"i have learned the person whit ID : "
+ str(userdata.in_learn_person)+ENDC)
return 'succeeded'
userdata.in_learn_person=None
return 'aborted'
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'})
