def execute(self, userdata): print "Dummy state to go to produce dummy data!" # in seconds rospy.sleep(0.5) userdata.from_Dummy_person_Id = 1 userdata.from_Dummy_all_Person_Data = peopleTracking() return succeeded
def execute(self, userdata): #UserdData Init rospy.loginfo('Executing ::PeopleTrackingProcessData::') userdata.personId = 101 userdata.all_person_data = peopleTracking() rospy.loginfo('###############Numero Personas detectadas %i' % len(userdata.process_peopleSet.peopleSet)) #We check there's someone if len(userdata.process_peopleSet.peopleSet) > 0: numero_personas_detectadas = len(userdata.process_peopleSet.peopleSet) rospy.loginfo('Numero Personas detectadas %i' % numero_personas_detectadas) i = 0 found_person = False min_distance = MAX_DISTANCE for data_peopleSet in userdata.process_peopleSet.peopleSet: #Now we make the calculations for knowing the Displecement in Y-Axis #And the distance magnitude. p = Pose() p.position.x = data_peopleSet.x p.position.y = data_peopleSet.y distance = vector_magnitude(p.position) displace = abs(p.position.y) rospy.loginfo('::ID = %i::\nDistance VS MAX_DISTANCE ::%f = %f::,\ndisplace VS MAX_DISPLACE ::%f = %f::' % (data_peopleSet.targetId, distance, MAX_DISTANCE, displace, MAX_DISPLACE)) #We only select who has the minimum distance. The displacement is just a filter. if displace < MAX_DISPLACE: if distance < min_distance: min_distance = distance data_personId = data_peopleSet.targetId rospy.loginfo('PLAUSIBLE CANDIDATE FOUND,With ID ::%i::,\nAt distance ::%f::,\nDisplacement ::%f:: ' % (data_personId, min_distance, displace)) all_per_data = data_peopleSet rospy.loginfo('ALL PLAUSIBLE PERSON DATA \n%s' % str(all_per_data)) found_person = True else: rospy.loginfo('NOT PLAUSIBLE CANDIDATE with ID = %i\nNo person close enough to the robot or \nWe found a person closer' % (data_peopleSet.targetId)) else: rospy.loginfo('NOT PLAUSIBLE CANDIDATE with ID = %i\nBecause no person infront of the robot or \nPerson not centered' % (data_peopleSet.targetId)) i += 1 if found_person: userdata.personId = data_personId userdata.all_person_data = all_per_data return succeeded rospy.loginfo('NO PEOPLE NO PEOPLE NO PEOPLE NO PEOPLE') rospy.sleep(SLEEP_TIME_WHEN_FAILS) return faulty_peopletrack