def people_callback(self, ros_data):
self.lock.acquire()
send_time = ros_data.header.stamp
idx = -1
max_distance = {}
for person in ros_data.people:
idx += 1
max_distance[str(idx)] = person.position.z
# print ">> Persons found {idx, distance}: ", max_distance
sort = sorted(max_distance.items(), key=operator.itemgetter(1), reverse=True)
# print ">> Nearest Face: ", sort
# print ">> Index: ", sort[0][0]
# print ">> Distance in pixels: ", sort[0][1]
self.nearest_person_x = ros_data.people[int(sort[0][0])].position.x
self.nearest_person_y = ros_data.people[int(sort[0][0])].position.y
self.nearest_person_z = ros_data.people[int(sort[0][0])].position.z
# print ">> Position in pixels x:", self.nearest_person_x
# print ">> Position in pixels y:", self.nearest_person_y
point = [self.nearest_person_x, self.nearest_person_y]
# Derive coordinate mapping
angles = self.trans.derive_mapping_coords(point)
if angles is not None:
g = RobotGaze()
if self.mode == 'relative':
g.gaze_type = RobotGaze.GAZETARGET_RELATIVE
else:
g.gaze_type = RobotGaze.GAZETARGET_ABSOLUTE
self.current_robot_gaze_timestamp = send_time.to_sec()
g.gaze_timestamp = RobotTimestamp(self.current_robot_gaze_timestamp)
g.pan = angles[0]
g.tilt = angles[1]
self.current_robot_gaze = g
self.lock.release()
self.honor_stimulus_timeout()
def point_callback(self, ros_data):
self.lock.acquire()
send_time = ros_data.header.stamp
self.point_x = ros_data.point.x
self.point_y = ros_data.point.y
self.point_z = ros_data.point.z
point = [self.point_x, self.point_y]
# Derive coordinate mapping
angles = self.trans.derive_mapping_coords(point)
if angles is not None:
g = RobotGaze()
if self.mode == 'absolute':
g.gaze_type = RobotGaze.GAZETARGET_ABSOLUTE
else:
g.gaze_type = RobotGaze.GAZETARGET_RELATIVE
self.current_robot_gaze_timestamp = send_time.to_sec()
g.gaze_timestamp = RobotTimestamp(
self.current_robot_gaze_timestamp)
g.pan = angles[0]
g.tilt = angles[1]
self.current_robot_gaze = g
self.lock.release()
self.honor_stimulus_timeout()
def people_callback(self, ros_data):
self.lock.acquire()
send_time = ros_data.header.stamp
idx = -1
max_distance = {}
for person in ros_data.people:
idx += 1
max_distance[str(idx)] = person.position.z
# print ">> Persons found {idx, distance}: ", max_distance
sort = sorted(max_distance.items(),
key=operator.itemgetter(1),
reverse=True)
# print ">> Nearest Face: ", sort
# print ">> Index: ", sort[0][0]
# print ">> Distance in pixels: ", sort[0][1]
self.nearest_person_x = ros_data.people[int(sort[0][0])].position.x
self.nearest_person_y = ros_data.people[int(sort[0][0])].position.y
self.nearest_person_z = ros_data.people[int(sort[0][0])].position.z
# print ">> Position in pixels x:", self.nearest_person_x
# print ">> Position in pixels y:", self.nearest_person_y
point = [self.nearest_person_x, self.nearest_person_y]
# Derive coordinate mapping
angles = self.trans.derive_mapping_coords(point)
if angles is not None:
g = RobotGaze()
if self.mode == 'relative':
g.gaze_type = RobotGaze.GAZETARGET_RELATIVE
else:
g.gaze_type = RobotGaze.GAZETARGET_ABSOLUTE
self.current_robot_gaze_timestamp = send_time.to_sec()
g.gaze_timestamp = RobotTimestamp(
self.current_robot_gaze_timestamp)
g.pan = angles[0]
g.tilt = angles[1]
self.current_robot_gaze = g
self.lock.release()
self.honor_stimulus_timeout()
def point_callback(self, ros_data):
self.lock.acquire()
send_time = ros_data.header.stamp
self.point_x = ros_data.point.x
self.point_y = ros_data.point.y
self.point_z = ros_data.point.z
point = [self.point_x, self.point_y]
# Derive coordinate mapping
angles = self.trans.derive_mapping_coords(point)
if angles is not None:
g = RobotGaze()
if self.mode == 'absolute':
g.gaze_type = RobotGaze.GAZETARGET_ABSOLUTE
else:
g.gaze_type = RobotGaze.GAZETARGET_RELATIVE
self.current_robot_gaze_timestamp = send_time.to_sec()
g.gaze_timestamp = RobotTimestamp(self.current_robot_gaze_timestamp)
g.pan = angles[0]
g.tilt = angles[1]
self.current_robot_gaze = g
self.lock.release()
self.honor_stimulus_timeout()
