| 18 | |
| 19 | |
| 20 | class FaceSaverNode: |
| 21 | def __init__(self, sensor_names): |
| 22 | self.syncs = [] |
| 23 | self.id_table = {} |
| 24 | self.bridge = cv_bridge.CvBridge() |
| 25 | |
| 26 | for sensor_name in sensor_names: |
| 27 | try: |
| 28 | rospy.client.wait_for_message('/%s/hd/image_color' % sensor_name, Image, 1.0) |
| 29 | image_sub = message_filters.Subscriber('/%s/hd/image_color' % sensor_name, Image) |
| 30 | except: |
| 31 | image_sub = message_filters.Subscriber('/%s/hd/image_color/compressed' % sensor_name, CompressedImage) |
| 32 | |
| 33 | self.subscribers = [ |
| 34 | image_sub, |
| 35 | message_filters.Subscriber('/%s/face_detector/detections' % sensor_name, DetectionArray), |
| 36 | message_filters.Subscriber('/face_feature_extractor/features', FeatureVectorArray), |
| 37 | message_filters.Subscriber('/tracker/association_result', Association) |
| 38 | ] |
| 39 | self.syncs.append(recutils.TimeSynchronizer(self.subscribers, 8192, 1000)) |
| 40 | self.syncs[-1].registerCallback(self.callback) |
| 41 | |
| 42 | self.subscribers2 = [ |
| 43 | message_filters.Subscriber('/tracker/tracks_smoothed', TrackArray), |
| 44 | message_filters.Subscriber('/face_recognition/people_tracks', TrackArray) |
| 45 | ] |
| 46 | self.sync2 = message_filters.TimeSynchronizer(self.subscribers2, 1024) |
| 47 | self.sync2.registerCallback(self.track_callback) |
| 48 | |
| 49 | def track_callback(self, track_msg, face_track_msg): |
| 50 | if len(track_msg.tracks) != len(face_track_msg.tracks): |
| 51 | return |
| 52 | |
| 53 | for i in range(len(track_msg.tracks)): |
| 54 | track = track_msg.tracks[i] |
| 55 | ftrack = face_track_msg.tracks[i] |
| 56 | if ftrack.id < 10000: |
| 57 | self.id_table[track.id] = ftrack.id |
| 58 | |
| 59 | def callback(self, img_msg, detection_msg, feature_msg, association_msg): |
| 60 | results = [] |
| 61 | for detection, feature, tracker_id in zip(detection_msg.detections, feature_msg.vectors, association_msg.track_ids): |
| 62 | if len(feature.data) < 5: |
| 63 | continue |
| 64 | |
| 65 | box = detection.box_2D |
| 66 | if box.width <= 0: |
| 67 | continue |
| 68 | |
| 69 | if tracker_id not in self.id_table: |
| 70 | continue |
| 71 | |
| 72 | results.append([box, tracker_id, self.id_table[tracker_id]]) |
| 73 | |
| 74 | if len(results) == 0: |
| 75 | return |
| 76 | |
| 77 | if type(img_msg) is Image: |