def execute(self, userdata):
rospy.loginfo('Calculating Vectors for 50cm distance ;P')
distance = 0.5
pose = userdata.FO_people_location
unit_vector = normalize_vector(pose.position)
k = vector_magnitude(pose.position)
distance_des = k - distance
dist_vector = multiply_vector(unit_vector, distance_des)
alfa = math.atan2(dist_vector.x, dist_vector.y)
if alfa > math.pi:
alfa = alfa - (2 * math.pi)
alfa_degree = (alfa * 360) / (2 * math.pi)
rospy.loginfo('$$$$$$$$$$$$$Real Distance:%s', str(k))
rospy.loginfo('$$$$$$$$$$$$$Desired Distance:%s', str(distance_des))
rospy.loginfo('$$$$$$$$$$$$$ALFA ==> :%s', str(alfa_degree))
nav_goal = PoseStamped()
nav_goal.header.stamp = rospy.Time.now()
nav_goal.header.frame_id = "/base_link"
nav_goal.pose.position = dist_vector
nav_goal.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, alfa))
userdata.l50_navgoal = nav_goal
rospy.loginfo('This is the Nav Goal We send to REEM: %s', str(nav_goal))
userdata.l50_lost_person = False
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
def execute(self, userdata):
rospy.loginfo('Calculating Vectors for 200cm distance ;P')
distance = 2.0
pose = userdata.sm_FO_old_pos
unit_vector = normalize_vector(pose.position)
k = vector_magnitude(pose.position)
distance_des = k + distance
dist_vector = multiply_vector(unit_vector, distance_des)
rospy.loginfo('$$$$$$$$$$$$$Real Distance:%s', str(k))
rospy.loginfo('$$$$$$$$$$$$$Desired Distance:%s', str(distance_des))
nav_goal = PoseStamped()
nav_goal.header.stamp = rospy.Time.now()
nav_goal.header.frame_id = "/base_link"
nav_goal.pose.position = dist_vector
nav_goal.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 0))
userdata.l200_navgoal = nav_goal
rospy.loginfo('This is the Nav Goal We send to REEM: %s', str(nav_goal))
userdata.l200_lost_person = True
userdata.l200_no_people_counter = NUMBER_TIMES_AFTER_LOST_BEFORE_LOOSING_AGAIN
return succeeded
def showPeopleTrackerData(userdata):
rospy.loginfo(colors.YELLOW + "Showing people tracker data..." + colors.NATIVE_COLOR)
global tracked_person_id
if (tracked_person_id is None):
pt_learning_zone_marker_pub = rospy.Publisher("/LEARNING_ZONE_people_tracker_test/", Marker)
learning_zone_marker = Marker()
learning_zone_marker.header.frame_id = "/base_link"
learning_zone_marker.ns = "learning_zone_ns"
learning_zone_marker.id = 0
learning_zone_marker.type = learning_zone_marker.LINE_STRIP
learning_zone_marker.action = learning_zone_marker.ADD
learning_zone_marker.points = [Point(), Point(), Point(), Point()]
learning_zone_marker.points[0].x = 0.0
learning_zone_marker.points[0].y = 0.0
learning_zone_marker.points[0].z = 0.0
learning_zone_marker.points[1].x = MAX_LEARN_DISTANCE
learning_zone_marker.points[1].y = MAX_LEARN_DISPLACEMENT
learning_zone_marker.points[1].z = 0.0
learning_zone_marker.points[2].x = MAX_LEARN_DISTANCE
learning_zone_marker.points[2].y = -MAX_LEARN_DISPLACEMENT
learning_zone_marker.points[2].z = 0.0
learning_zone_marker.points[3].x = 0.0
learning_zone_marker.points[3].y = 0.0
learning_zone_marker.points[3].z = 0.0
learning_zone_marker.pose.position.x = 0.0
learning_zone_marker.pose.position.y = 0.0
learning_zone_marker.pose.position.z = 0.0
learning_zone_marker.scale.x = 0.05
learning_zone_marker.color.a = 0.9
learning_zone_marker.color.r = 1.0
learning_zone_marker.color.g = 0.5
learning_zone_marker.color.b = 0.4
learning_zone_marker.lifetime = rospy.Duration(1.0)
learning_zone_marker.header.stamp = rospy.Time.now()
pt_learning_zone_marker_pub.publish(learning_zone_marker)
min_distance = MAX_LEARN_DISTANCE
for person in userdata.in_persons_detected.peopleSet:
displace = abs(person.y)
person_pose = Pose()
person_pose.position.x = person.x
person_pose.position.y = person.y
distance = vector_magnitude(person_pose.position)
if displace < MAX_LEARN_DISPLACEMENT and distance < min_distance:
min_distance = distance
tracked_person_id = person.targetId
if (tracked_person_id is None):
rospy.loginfo(colors.YELLOW_BOLD + " THERE ISN'T ANY CANDIDATE IN FRONT OF THE ROBOT" + colors.NATIVE_COLOR)
else:
rospy.loginfo(colors.BACKGROUND_GREEN + " Following person with id => " + str(tracked_person_id) + colors.NATIVE_COLOR)
tracked_person = findPersonByIdInPeopleSet(tracked_person_id, userdata.in_persons_detected.peopleSet)
if (tracked_person is not None):
pt_tracked_marker_pub = rospy.Publisher("/LEARNING_ZONE_people_tracker_test/", Marker)
marker = Marker()
marker.header.frame_id = "/base_link"
marker.ns = "person_tracked_ns"
marker.id = 0
marker.type = marker.CYLINDER
marker.action = marker.ADD
marker.pose.position.x = tracked_person.x
marker.pose.position.y = tracked_person.y
marker.pose.position.z = 0.5
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 1.0
marker.color.a = 1.0
marker.color.r = 0.2
marker.color.g = 0.2
marker.color.b = 0.2
marker.lifetime = rospy.Duration(10.0)
marker.header.stamp = rospy.Time.now()
pt_tracked_marker_pub.publish(marker)
elif (tracked_person_id is not None):
rospy.loginfo(colors.RED_BOLD + " LOST PERSON with id => " + str(tracked_person_id) + colors.NATIVE_COLOR)
pt_test_marker_pub = rospy.Publisher("/people_tracker_test/", MarkerArray)
global markers
global i
for person in userdata.in_persons_detected.peopleSet:
marker = Marker()
marker.header.frame_id = "/base_link"
marker.ns = "people_tracked_ns"
marker.id = i
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points = [Point(), Point()]
marker.points[0].x = person.x
marker.points[0].y = person.y
marker.points[0].z = 0.0
marker.points[1].x = person.x + person.vx
marker.points[1].y = person.y + person.vy
marker.points[1].z = 0.0
marker.scale.x = 0.1 # arrow shaft diameter
marker.scale.y = 0.1 # arrow head diameter
marker.scale.z = 0.0 # arrow head length if is not zero
marker.color.a = 0.4
marker.color.r = 0.5
marker.color.g = 0.0
marker.color.b = 1.0
marker.lifetime = rospy.Duration(PT_TEST_MARKERS_LIFETIME)
marker.header.stamp = rospy.Time.now()
marker_array.markers.append(marker)
i += 1
if (i >= MAX_NUMBER_PT_TEST_MARKERS):
marker_array.markers.pop(0)
pt_test_marker_pub.publish(marker_array)
return succeeded
def execute(self, userdata):
rospy.loginfo(':: Calculating Vectors for %s m distance ::', str(self._distance))
#self._distance
pose = userdata.FO_people_location
unit_vector = normalize_vector(pose.position)
rospy.loginfo("pose.position is:\n" + str(pose.position))
rospy.loginfo("unit_vector is:\n " + str(unit_vector))
k = vector_magnitude(pose.position)
rospy.loginfo("Magnitude of vector from Reem to person ==> " + str(k))
if not self._we_lost_person:
"""
If person is closer than the distance given, we wont move but we might rotate.
We want that if the person comes closer, the robot stays in the place.
Thats why we make desired distance zero if person too close.
"""
if k >= self._distance:
distance_des = k - self._distance
else:
distance_des = 0.0
else:
distance_des = k + self._distance
dist_vector = multiply_vector(unit_vector, distance_des)
alfa = 0.0
"""
We only calculate the turning if there is no base movement
"""
if distance_des == 0.0:
alfa = math.atan2(pose.position.y, pose.position.x)
rospy.loginfo("ALFA is: " + str(alfa) + " in degrees: " + str(math.degrees(alfa)))
if alfa > math.pi:
rospy.loginfo("PERSON DETECTED BEHIND REEM, IMPOSSIBLE: " + str(alfa) + " in degrees: " + str(math.degrees(alfa)))
else:
rospy.loginfo("We will move so we WONT TURN, DESIRED_DISTANCE is > 0 ==> " + str(distance_des))
alfa_degree = math.degrees(alfa)
print "/********************************************/"
rospy.loginfo('Distance from PERSON:%s', str(k))
rospy.loginfo('Person and REEM WANTED distance:%s', str(self._distance))
rospy.loginfo('Distance that Reem will MOVE:%s', str(distance_des))
rospy.loginfo('Degrees that Reem will TURN ==> :%s', str(alfa_degree))
print "/********************************************/"
nav_goal = PoseStamped()
nav_goal.header.stamp = rospy.Time.now()
nav_goal.header.frame_id = "/base_link"
nav_goal.pose.position = dist_vector
nav_goal.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, alfa))
rospy.loginfo('ALFA AND ORIENTATION ==> :%s, %s', str(alfa_degree), str(nav_goal.pose.orientation))
userdata.l_navgoal = nav_goal
rospy.loginfo('This is the Nav Goal We send to REEM: %s', str(nav_goal))
if not self._we_lost_person:
userdata.l_lost_person = False
else:
userdata.l_lost_person = True
userdata.l_no_people_counter = NUMBER_TIMES_AFTER_LOST_BEFORE_LOOSING_AGAIN
return succeeded
