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