class RecognitionSubscriber(object):
'''
Class that subscribes on compute_faces_and_position
receives images and computes the face id and target location from this
and publishes this on the people_simple topic
'''
def __init__(self):
'''
This class does not take any arguments
'''
self.fr = FaceRecognizer()
self.bridge = CvBridge()
rospy.Subscriber('/odom', Odometry, self.__odom_callback)
# Subscribe on the received images
rospy.Subscriber('/custom/compute_faces_and_positions', Image,
self.__request_callback)
# Pubish the computed information
self.recognition_pub = rospy.Publisher('/custom/people_simple',
Float32MultiArray,
queue_size=1)
self.position_publisher = rospy.Publisher('/custom/people_positions',
PoseStamped,
queue_size=1,
latch=True)
# Contineously keep track of the current odometry to calculate target location
# in correspondance to odom
self.odom = Odometry()
# Keep lists of the recognized ids, locations and times
# lists are easy to remove a target id from all lists after a
# while and to check if id is present, ...
self.recognized_ids = []
self.recognized_locs = []
self.recognized_times = []
def publish_position(self, x, y, frame_id):
'''
Publish the position of a detected face on the map
arguments: x, y, frame_id (reference)
'''
pose = PoseStamped()
pose.header.frame_id = frame_id
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = x
pose.pose.position.y = y
self.position_publisher.publish(pose)
def __request_callback(self, msg):
'''
Callback if image is received, faces are detected and
added to the global array of people
A message is sent with the ids, locations and face widths
in a multiarray.
'''
cv_image = self.bridge.imgmsg_to_cv2(msg, 'rgb8')
if SHOW_RECEIVED_IMAGE:
cv2.imwrite('rec_img.png', cv_image)
ret = self.fr.recognize_people(cv_image)
simple_msg = Float32MultiArray()
if ret:
print('new face detected')
locs, ids = ret
for i, loc in zip(ids, locs):
if i in self.recognized_ids:
index = self.recognized_ids.index(i)
self.recognized_locs[index] = loc
self.recognized_times[index] = datetime.datetime.now()
else:
self.recognized_ids.append(i)
self.recognized_locs.append(loc)
self.recognized_times.append(datetime.datetime.now())
else:
print('No faces detected')
# Publish the computed info on the simple_people topic
# as a multiarray. The structure is defined as [id, xpos, ypos, width]
# in a repeated fashion for multiple faces.
# The width might be used as a metric for how far the face is away
dim = MultiArrayDimension()
dim.label = 'id_and_pos'
dim.size = 3
dim.stride = 0
dim2 = MultiArrayDimension()
dim2.label = 'length_faces'
dim2.size = len(self.recognized_ids)
dim2.stride = 0
simple_msg.layout.dim = [dim, dim2]
simple_msg.layout.data_offset = 0
data = []
# Add the recognized faces around the table to the message
for i, loc in zip(self.recognized_ids, self.recognized_locs):
width = self.__get_width(loc)
x, y, valid = self.__calculate_people_positions(width)
data = np.concatenate((data, [i, x, y, width]), axis=0)
simple_msg.data = data
print(simple_msg)
# Publish the message
self.recognition_pub.publish(simple_msg)
# Check if person is seen in the last TIME_BEFORE_FORGOTTEN seconds and otherwise remove
for index, time in enumerate(self.recognized_times):
if abs((time - datetime.datetime.now()
).total_seconds()) > TIME_BEFORE_FORGOTTEN:
del self.recognized_ids[index]
del self.recognized_locs[index]
del self.recognized_times[index]
def __odom_callback(self, msg):
'''
Keep the odometry up to date
'''
self.odom = msg
def __get_width(self, location):
'''
Helper function to calculate the width of a face
'''
return abs(location[3] - location[1])
def __get_height(self, location):
'''
Helper function to calculate the height of a face
'''
return abs(location[0] - location[2])
def __get_center(self, location):
'''
Helper function to calculate the center of a face
'''
def get_top_left(location):
return location[3], location[0]
def get_bottom_right(location):
return location[1], location[2]
# Calculate the x, y position of the found person
x_l, y_l = get_top_left(location)
x_r, y_r = get_bottom_right(location)
point = Point()
point.x = (x_l + x_r) / 2.0
point.y = (y_l + y_r) / 2.0
point.z = 0
return point
def __calculate_people_positions(self, width):
'''
Calculate the peoples position based on the current odometry
Assume that the face is straight ahead and the distance is
dependent on the face width
'''
_, _, yaw = tf.transformations.euler_from_quaternion([
self.odom.pose.pose.orientation.x,
self.odom.pose.pose.orientation.y,
self.odom.pose.pose.orientation.w,
self.odom.pose.pose.orientation.z,
])
print(yaw)
print(self.odom)
target_pose = copy.deepcopy(self.odom)
#fac = 1.0/width
offset = 0.10
fac = max(-1.36 * np.log10(width) + 3.63, 0)
#fac = 0.3
print(fac)
x = target_pose.pose.pose.position.x + fac - offset #np.cos(yaw) * fac * width
y = target_pose.pose.pose.position.y #- np.sin(yaw) * fac * width
#self.publish_position(x, y, 'base_link')
listener = tf.TransformListener()
listener.waitForTransform("/base_link", "/odom", rospy.Time(0),
rospy.Duration(4.0))
laser_point = PointStamped()
laser_point.header.frame_id = "base_link"
laser_point.header.stamp = rospy.Time(0)
laser_point.point.x = x
laser_point.point.y = y
laser_point.point.z = 0.0
p = listener.transformPoint("odom", laser_point)
x = p.point.x
y = p.point.y
valid = fac < 1.6
#new_vec = transform(np.asarray([x,y,0]), 'base_link', 'odom')
#x = new_vec[0]
#y = new_vec[1]
self.publish_position(x, y, 'odom')
return x, y, valid
class RecognitionServer(object):
'''
In class so that the same face recognizer class is used
for all images (Ids and face embeddings are stored there)
# --
When receiving a request, it publishes a simple solution on '/custom/people_simple'
'''
def __init__(self):
self.fr = FaceRecognizer()
self.bridge = CvBridge()
self.odom_sub = rospy.Subscriber('/odom', Pose, self.__odom_callback)
self.odom = Pose()
self.recognition_pub = rospy.Publisher('/custom/people_simple',
Float32MultiArray,
queue_size=1)
def __odom_callback(self, msg):
self.odom = msg
def __get_center(self, location):
def get_top_left(location):
return location[3], location[0]
def get_bottom_right(location):
return location[1], location[2]
# Calculate the x, y position of the found person
x_l, y_l = get_top_left(location)
x_r, y_r = get_bottom_right(location)
point = Point()
point.x = (x_l + x_r) / 2.0
point.y = (y_l + y_r) / 2.0
point.z = 0
return point
def __calculate_people_positions(self):
_, _, yaw = tf.transformations.euler_from_quaternion([
self.odom.pose.orientation.x,
self.odom.pose.orientation.y,
self.odom.pose.orientation.w,
self.odom.pose.orientation.z,
])
target_pose = copy.deepcopy(self.odom)
target_pose.position.x += np.sin(yaw)
target_pose.position.x += np.cos(yaw)
return target_pose.x, target_pose.y
def handle_service_request(self, request):
cv_image = self.bridge.imgmsg_to_cv2(request.img, 'bgr8')
ret = self.fr.recognize_people(cv_image)
if ret:
locs, ids = ret
# Publish simple info on the simple_people topic
simple_msg = Float32MultiArray()
dim = MultiArrayDimension()
dim.label = 'id_and_pos'
dim.size = 3
dim.stride = 0
dim2 = MultiArrayDimension()
dim2.label = 'length_faces'
dim2.size = len(ids)
dim2.stride = 0
simple_msg.layout.dim.label = [dim, dim2]
simple_msg.data_offset = 0
data = []
for i in ids:
x, y = self.__calculate_people_positions()
np.concatenate(data, [i, x, y])
simple_msg.data = data
print(len(data), 3 * len(ids))
print(simple_msg)
# publish the simple message
self.recognition_pub(simple_msg)
# Handle service request
message = DetectFaceResponse()
message.ids = ids
message.positions = [self.__get_center(loc) for loc in locs]
return message
else:
return DetectFaceResponse()
