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
