def ros_deepsort_callback(self, ros_data):
start = time.time()
#convert ros compressed image message to opencv
if self.args.service:
np_arr = np.fromstring(ros_data.img.data, np.uint8)
elif self.args.img_topic:
np_arr = np.fromstring(ros_data.data, np.uint8)
ori_im = cv2.imdecode(np_arr, flags=cv2.IMREAD_COLOR)
im = cv2.cvtColor(ori_im, cv2.COLOR_BGR2RGB)
#skip frame per frame interval
self.idx_frame += 1
if self.idx_frame % self.args.frame_interval:
return
# do detection
bbox_xywh, cls_conf, cls_ids = self.detector(im)
# select person class
mask = cls_ids == 0
bbox_xywh = bbox_xywh[mask]
# bbox dilation just in case bbox too small, delete this line if using a better pedestrian detector
bbox_xywh[:, 3:] *= 1.2
cls_conf = cls_conf[mask]
# do tracking
outputs = self.deepsort.update(bbox_xywh,
cls_conf,
im,
tracking_target=self.idx_tracked)
# if detection present draw bounding boxes
if len(outputs) > 0:
bbox_tlwh = []
self.bbox_xyxy = outputs[:, :4]
# detection indices
self.identities = outputs[:, -1]
ori_im = draw_boxes(ori_im, self.bbox_xyxy, self.identities)
for bb_xyxy in self.bbox_xyxy:
bbox_tlwh.append(self.deepsort._xyxy_to_tlwh(bb_xyxy))
end = time.time()
#draw frame count
font = cv2.FONT_HERSHEY_SIMPLEX
bottomLeftCornerOfText = (10, 500)
fontScale = 1
fontColor = (255, 255, 255)
lineType = 2
frame_count = ("Frame no: %d" % self.idx_frame)
cv2.putText(ori_im, frame_count, bottomLeftCornerOfText, font,
fontScale, fontColor, lineType)
#draw tracking number
if self.idx_tracked:
tracking_str = ("Tracking: %d" % self.idx_tracked)
else:
tracking_str = ("Tracking: None")
bottomLeftCornerOfText = (10, 550)
cv2.putText(ori_im, tracking_str, bottomLeftCornerOfText, font,
fontScale, fontColor, lineType)
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', ori_im)[1]).tostring()
# Publish new image
self.image_pub.publish(msg)
if self.args.save_results:
self.writer.write(ori_im)
# logging
self.logger.info("frame: {}, time: {:.03f}s, fps: {:.03f}, detection numbers: {}, tracking numbers: {}" \
.format(self.idx_frame, end-start, 1/(end-start), bbox_xywh.shape[0], len(outputs)))
"""publishing to topics"""
#publish detection identities
identity_msg = Int32MultiArray(data=self.identities)
self.detections_pub.publish(identity_msg)
#publish if target present
if len(outputs) == 0 or self.idx_tracked is None:
self.target_present_pub.publish(Bool(False))
elif len(outputs) > 0 and self.idx_tracked:
self.target_present_pub.publish(Bool(True))
#publish angle and xy data
if self.idx_tracked is not None:
x_center = (self.bbox_xyxy[0][0] + self.bbox_xyxy[0][2]) / 2
y_center = (self.bbox_xyxy[0][1] + self.bbox_xyxy[0][3]) / 2
pixel_per_angle = im.shape[1] / self.camera_fov
x_center_adjusted = x_center - (im.shape[1] / 2)
#print(x_center_adjusted)
angle = x_center_adjusted / pixel_per_angle
#print(angle)
if self.args.img_topic:
self.bbox_pub.publish(x_center, y_center, 0)
self.angle_pub.publish(angle)
self.target_indice_pub.publish(Int32(self.idx_tracked))
msg = Point()
msg.x = x_center
msg.y = y_center
msg.z = 0
return msg
#publish if target initialized
else:
self.target_indice_pub.publish(Int32(self.idx_tracked))
msg = Point()
msg.x = 0
msg.y = 0
msg.z = 0
return msg
