| 31 | # then, it outputs the modified DetectionArray as an array of face detection results |
| 32 | # note that the replaced box_2d is on the color image coordinate |
| 33 | class FaceDetectionNode: |
| 34 | def __init__(self, sensor_name): |
| 35 | self.sensor_name = sensor_name |
| 36 | |
| 37 | self.cfg_server = Server(FaceDetectionConfig, self.cfg_callback) |
| 38 | self.cv_bridge = cv_bridge.CvBridge() |
| 39 | self.detector = dlib.fhog_object_detector(self.face_detector_path) |
| 40 | self.pool = multiprocessing.Pool(3) |
| 41 | |
| 42 | # get transformation between world, color, and depth images |
| 43 | now = rospy.Time(0) |
| 44 | tf_listener = tf.TransformListener() |
| 45 | print self.sensor_name |
| 46 | self.ir2rgb = recutils.lookupTransform(tf_listener, self.sensor_name + '_ir_optical_frame', self.sensor_name + '_rgb_optical_frame', 10.0, now) |
| 47 | # self.ir2rgb = numpy.eye(4, 4).astype(numpy.float64) |
| 48 | print '--- ir2rgb ---\n', self.ir2rgb |
| 49 | self.world2rgb = recutils.lookupTransform(tf_listener, '/world', self.sensor_name + '_rgb_optical_frame', 10.0, now) |
| 50 | print '--- world2rgb ---\n', self.world2rgb |
| 51 | |
| 52 | self.pub = rospy.Publisher('/face_detector/detections', DetectionArray, queue_size=10) |
| 53 | self.pub_local = rospy.Publisher(self.sensor_name + '/face_detector/detections', DetectionArray, queue_size=10) |
| 54 | |
| 55 | try: |
| 56 | print 'tryingnsecs_round to listen raw rgb image topic...' |
| 57 | rospy.client.wait_for_message(self.sensor_name + '/hd/image_color', Image, 1.0) |
| 58 | img_subscriber = message_filters.Subscriber(self.sensor_name + '/hd/image_color', Image) |
| 59 | except rospy.ROSException: |
| 60 | print 'failed, listen compressed rgb image topic' |
| 61 | img_subscriber = message_filters.Subscriber(self.sensor_name + '/hd/image_color/compressed', CompressedImage) |
| 62 | |
| 63 | self.subscribers = [ |
| 64 | img_subscriber, |
| 65 | message_filters.Subscriber(self.sensor_name + '/hd/camera_info', CameraInfo), |
| 66 | message_filters.Subscriber('/detector/detections', DetectionArray) |
| 67 | ] |
| 68 | |
| 69 | # TypeSynchronizer doesn't work, the image time and the detection time are slightly different? |
| 70 | # self.ts = message_filters.TimeSynchronizer(self.subscribers, 5) |
| 71 | # self.ts = message_filters.ApproximateTimeSynchronizer(self.subscribers, 5, 0.0001) |
| 72 | self.ts = recutils.TimeSynchronizer(self.subscribers, 60, 1000) |
| 73 | self.ts.registerCallback(self.callback) |
| 74 | |
| 75 | self.reset_time_sub = rospy.Subscriber('/reset_time', Empty, self.reset_time) |
| 76 | print("init complete") |
| 77 | |
| 78 | # callback for dynamic configure |
| 79 | def cfg_callback(self, config, level): |
| 80 | package_path = rospkg.RosPack().get_path('recognition') |
| 81 | self.face_detector_path = package_path + config.face_detector_path # the path to the face detector model file |
| 82 | self.confidence_thresh = config.confidence_thresh # the threshold for confidence of face detection |
| 83 | self.roi_width = config.roi_width_ # the width of a face detection ROI in the world space [m] |
| 84 | self.calc_roi_from_top = config.calc_roi_from_top # if true, ROIs are calculated from the top positions of detected clusters |
| 85 | self.head_offset_z_top = config.head_offset_z_top # the distance between the top position of a human cluster and the center of the face [m] |
| 86 | self.head_offset_z_centroid = config.head_offset_z_centroid # the distance between the centroid of a human cluster and the center of the face [m] |
| 87 | self.upscale_minsize = config.upscale_minsize # the face detection ROI is upscaled so that its width get larger than #upscale_minsize |
| 88 | self.visualization = config.visualization # if true, the visualization of the detection will be shown |
| 89 | |
| 90 | print '--- cfg_callback ---' |