Aws face update

doc/jsk_perception/nodes/aws_auto_checkin_app.rst

@@ -49,6 +49,11 @@ Publishing Topic
 
   Detected face class labels and probabilities.
 
+* ``~image`` (``sensor_msgs/Image``)
+
+  Passthourgh input image. This message contains face detected image
+  when ``~always_publish`` is false.
+
 Parameters
 ----------
 
@@ -110,6 +115,9 @@ Parameters
       "aws_secret_access_key" : "********************"
   }
 
+* ``~always_publish`` (Bool, Default: ``True``)
+
+  Set false to publish only when face is detected.
 
 Sample
 ------

doc/jsk_perception/nodes/aws_detect_faces.rst

@@ -61,6 +61,11 @@ Publishing Topic
 
   Visualization image of detected faces.
 
+* ``~image`` (``sensor_msgs/Image``)
+
+  Passthourgh input image. This message contains face detected image
+  when ``~always_publish`` is false.
+
 
 Parameters
 ----------
@@ -102,6 +107,10 @@ Parameters
 
   Set true to process even if not one subscribing.
 
+* ``~always_publish`` (Bool, Default: ``True``)
+
+  Set false to publish only when face is detected.
+
 Example
 -------
 

jsk_perception/node_scripts/aws_auto_checkin_app.py

@@ -87,6 +87,9 @@ def __init__(self):
             region_name=region_name)
         self.dynamodb_table = self.dynamodb.Table(self.DYNAMODB_TABLE)
 
+        self.always_publish = rospy.get_param('~always_publish', True)
+        rospy.loginfo("Publish even if face is not found : {}".format(self.always_publish))
+
         self.use_window = rospy.get_param('~use_window', False)
         rospy.loginfo("Launch image window : {}".format(self.use_window))
 
@@ -106,16 +109,31 @@ def __init__(self):
                                         queue_size=1)
         self.pub_class = self.advertise("~output/class", ClassificationResult,
                                         queue_size=1)
+        if self.transport_hint == 'compressed':
+            self.orig_image_pub = self.advertise('~image/compressed', CompressedImage, queue_size=1)
+        else:
+            self.orig_image_pub = self.advertise('~image', Image, queue_size=1)
+        #
+        # To process latest message, we need to set buff_size must be large enough.
+        # we need to set buff_size larger than message size to use latest message for callback
+        # 640*480(image size) / 5 (expected compressed rate) *
+        #            70 (number of message need to be drop 70 x 30msec = 2100msec processing time)
+        #
+        # c.f. https://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/
+        #
+        self.buff_size = rospy.get_param('~buff_size', 640 * 480 * 3 // 5 * 70)
+        rospy.loginfo("rospy.Subscriber buffer size : {}".format(self.buff_size))
 
     def subscribe(self):
         if self.transport_hint == 'compressed':
-            self.image_sub = message_filters.Subscriber('{}/compressed'.format(rospy.resolve_name('image')), CompressedImage)
+            self.image_sub = message_filters.Subscriber('{}/compressed'.format(rospy.resolve_name('image')), CompressedImage, buff_size=self.buff_size)
         else:
-            self.image_sub = message_filters.Subscriber('image', Image)
+            self.image_sub = message_filters.Subscriber('image', Image, buff_size=self.buff_size)
         self.roi_sub = message_filters.Subscriber('face_roi', FaceArrayStamped)
         self.subs = [self.image_sub, self.roi_sub]
-        queue_size = rospy.get_param('~queue_size', 100)
-        if rospy.get_param('~approximate_sync', True):
+        queue_size = rospy.get_param('~queue_size', 1)
+        approximate_sync = rospy.get_param('~approximate_sync', True)
+        if approximate_sync:
             slop = rospy.get_param('~slop', 1.0)
             self.ts = message_filters.ApproximateTimeSynchronizer(
                 self.subs,
@@ -124,6 +142,9 @@ def subscribe(self):
             self.ts = message_filters.TimeSynchronizer(
                 fs=self.subs, queue_size=queue_size)
         self.ts.registerCallback(self.callback)
+        rospy.loginfo("To process latest incomming message, use approximate_sync with queue_size == 1 is recommended")
+        rospy.loginfo("  approximate_sync : {}".format(approximate_sync))
+        rospy.loginfo("  queue_size : {}".format(queue_size))
         rospy.loginfo("Waiting for {} and {}".format(self.image_sub.name, self.roi_sub.name))
 
     def unsubscribe(self):
@@ -158,6 +179,7 @@ def findface(self, face_image):
         return None
 
     def callback(self, image, roi):
+        start_time = rospy.Time.now()
         if self.transport_hint == 'compressed':
             # decode compressed image
             np_arr = np.fromstring(image.data, np.uint8)
@@ -189,9 +211,13 @@ def callback(self, image, roi):
             ret = self.findface(img[image_roi_slice])
             if ret != None:
                 if ret['FaceMatches'] != []:
-                    face_id = self.dynamodb_table.get_item(
+                    item = self.dynamodb_table.get_item(
                         Key={'RekognitionId':
-                             ret['FaceMatches'][0]['Face']['FaceId']})['Item']['Name']
+                             ret['FaceMatches'][0]['Face']['FaceId']})
+                    if not 'Item' in item:
+                        rospy.loginfo("Item does not have FaceId {}".format(item))
+                        continue
+                    face_id = item['Item']['Name']
                     rospy.loginfo("FaceId: {}\n Similarity: {}".format(face_id, \
                                                                        ret['FaceMatches'][0]['Similarity']))
                     faces.faces.append(Face(face=Rect(cx - w // 2, cy - h // 2, w, h),
@@ -201,6 +227,10 @@ def callback(self, image, roi):
             if self.use_window: # copy colored face rectangle to img_gray
                 img_gray[image_roi_slice] = img[image_roi_slice]
 
+        # is always_publish is False, publish results only when the face is not detected
+        if not self.always_publish and len(faces.faces) <= 0:
+            return
+
         self.name_pub.publish(faces)
 
         cls_msg = ClassificationResult(
@@ -219,10 +249,17 @@ def callback(self, image, roi):
         self.pub_rects.publish(rects_msg)
         self.pub_class.publish(cls_msg)
 
+        if self.orig_image_pub.get_num_connections() > 0:
+            self.orig_image_pub.publish(image)
+
         if self.use_window:
             cv2.imshow(image._connection_header['topic'], img_gray)
             cv2.waitKey(1)
 
+        rospy.loginfo("processing time {} on message taken at {} sec ago".format(
+            (rospy.Time.now() - start_time).to_sec(),
+            (rospy.Time.now() - image.header.stamp).to_sec()))
+
 
 if __name__ == '__main__':
     rospy.init_node('aws_auto_checkin_service')

jsk_perception/node_scripts/aws_detect_faces.py

@@ -89,6 +89,9 @@ def __init__(self):
 
         self.bridge = cv_bridge.CvBridge()
 
+        self.always_publish = rospy.get_param('~always_publish', True)
+        rospy.loginfo("Publish even if face is not found : {}".format(self.always_publish))
+
         self.use_window = rospy.get_param('~use_window', False)
         rospy.loginfo("Launch image window : {}".format(self.use_window))
 
@@ -103,6 +106,8 @@ def __init__(self):
         self.attributes_pub = self.advertise('~attributes', ClassificationResult, queue_size=1)
         self.landmarks_pub = self.advertise('~landmarks', PeoplePoseArray, queue_size=1)
         self.image_pub = self.advertise('~output', Image, queue_size=1)
+        self.image_comp_pub = self.advertise('~output/compressed', CompressedImage, queue_size=1)
+        self.orig_image_pub = self.advertise('~image/compressed', CompressedImage, queue_size=1)
         #
         # To process latest message, we need to set buff_size must be large enough.
         # we need to set buff_size larger than message size to use latest message for callback
@@ -111,7 +116,7 @@ def __init__(self):
         #
         # c.f. https://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/
         #
-        self.buff_size = rospy.get_param('~buff_size', 640 * 480 // 5 * 10)
+        self.buff_size = rospy.get_param('~buff_size', 640 * 480 * 3 // 5 * 10)
         rospy.loginfo("rospy.Subscriber buffer size : {}".format(self.buff_size))
 
     def subscribe(self):
@@ -167,7 +172,8 @@ def process_attributes(self, text, img, bbox):
     @property
     def visualize(self):
         return self.use_window \
-            or self.image_pub.get_num_connections() > 0
+            or self.image_pub.get_num_connections() > 0 \
+            or self.image_comp_pub.get_num_connections() > 0
 
     def image_callback(self, image):
         start_time = rospy.Time.now()
@@ -355,17 +361,35 @@ def image_callback(self, image):
             cv2.imshow(image._connection_header['topic'], img_gray)
             cv2.waitKey(1)
 
+        # is always_publish is False, and face is not detected , do not publish any results
+        if not self.always_publish and len(faces['FaceDetails']) <= 0:
+            # debug info
+            rospy.loginfo("processing time {}".format((rospy.Time.now() - start_time).to_sec()))
+            return
+
         if self.image_pub.get_num_connections() > 0:
             self.image_pub.publish(self.bridge.cv2_to_imgmsg(
                 img_gray, encoding='bgr8'))
 
+        if self.image_comp_pub.get_num_connections() > 0:
+            msg = CompressedImage()
+            msg.header = image.header
+            msg.format = "jpeg"
+            msg.data = np.array(cv2.imencode('.jpg', img_gray)[1]).tostring()
+            self.image_comp_pub.publish(msg)
+
+        if self.orig_image_pub.get_num_connections() > 0:
+            self.orig_image_pub.publish(image)
+
         self.faces_pub.publish(face_msgs)
         self.poses_pub.publish(pose_msgs)
         self.attributes_pub.publish(attributes_msgs)
         self.landmarks_pub.publish(landmarks_msgs)
 
         # debug info
-        rospy.loginfo("processing time {}".format((rospy.Time.now() - start_time).to_sec()))
+        rospy.loginfo("processing time {} on message taken at {} sec ago".format(
+            (rospy.Time.now() - start_time).to_sec(),
+            (rospy.Time.now() - image.header.stamp).to_sec()))
 
 
 if __name__ == '__main__':

jsk_perception/node_scripts/draw_rects.py

@@ -27,15 +27,34 @@ def __init__(self):
         self.colors = np.array(np.clip(
             labelcolormap() * 255, 0, 255), dtype=np.uint8)
         self.subs = []
-        self.use_classification_result = DrawRectsConfig.defaults[
-            'use_classification_result']
-        self.approximate_sync = DrawRectsConfig.defaults['approximate_sync']
-        self.queue_size = DrawRectsConfig.defaults['queue_size']
+        self.use_classification_result = rospy.get_param('~use_classification_result', DrawRectsConfig.defaults[
+            'use_classification_result'])
+        self.approximate_sync = rospy.get_param('~approximate_sync', DrawRectsConfig.defaults['approximate_sync'])
+        self.queue_size = rospy.get_param('~queue_size', DrawRectsConfig.defaults['queue_size'])
+        self.transport_hint = rospy.get_param('~image_transport', 'raw')
+        rospy.loginfo("Using transport {}".format(self.transport_hint))
+        #
+        # To process latest message, we need to set buff_size must be large enough.
+        # we need to set buff_size larger than message size to use latest message for callback
+        # 640*480(image size) / 5 (expected compressed rate) *
+        #            70 (number of message need to be drop 70 x 30msec = 2100msec processing time)
+        #
+        # c.f. https://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/
+        #
+        self.buff_size = rospy.get_param('~buff_size', 640 * 480 * 3 // 5 * 70)
+        rospy.loginfo("rospy.Subscriber buffer size : {}".format(self.buff_size))
+
+        self.bridge = cv_bridge.CvBridge()
+
         self._srv_dynparam = dynamic_reconfigure.server.Server(
             DrawRectsConfig, self._config_callback)
 
-        self.pub_viz = self.advertise(
-            '~output', sensor_msgs.msg.Image, queue_size=1)
+        if self.transport_hint == 'compressed':
+            self.pub_viz = self.advertise(
+                '{}/compressed'.format(rospy.resolve_name('~output')), sensor_msgs.msg.CompressedImage, queue_size=1)
+        else:
+            self.pub_viz = self.advertise(
+                '~output', sensor_msgs.msg.Image, queue_size=1)
 
     def _config_callback(self, config, level):
         need_resubscribe = False
@@ -67,7 +86,10 @@ def _config_callback(self, config, level):
         return config
 
     def subscribe(self):
-        sub_image = message_filters.Subscriber('~input', sensor_msgs.msg.Image)
+        if self.transport_hint == 'compressed':
+            sub_image = message_filters.Subscriber('{}/compressed'.format(rospy.resolve_name('~input')), sensor_msgs.msg.CompressedImage, buff_size=self.buff_size)
+        else:
+            sub_image = message_filters.Subscriber('~input', sensor_msgs.msg.Image, buff_size=self.buff_size)
         sub_rects = message_filters.Subscriber('~input/rects', RectArray)
         warn_no_remap('~input', '~input/rects')
 
@@ -78,24 +100,33 @@ def subscribe(self):
             subs.append(sub_class)
 
         if self.approximate_sync:
-            slop = rospy.get_param('~slop', 0.1)
-            sync = message_filters.ApproximateTimeSynchronizer(
+            slop = rospy.get_param('~slop', 1.0)
+            self.sync = message_filters.ApproximateTimeSynchronizer(
                 subs,
                 queue_size=self.queue_size, slop=slop)
-            sync.registerCallback(self.draw_rects_callback)
+            self.sync.registerCallback(self.draw_rects_callback)
         else:
-            sync = message_filters.TimeSynchronizer(
+            self.sync = message_filters.TimeSynchronizer(
                 subs, queue_size=self.queue_size)
-            sync.registerCallback(self.draw_rects_callback)
+            self.sync.registerCallback(self.draw_rects_callback)
         self.subs = subs
+        rospy.loginfo("  approximate_sync : {}".format(self.approximate_sync))
+        rospy.loginfo("  queue_size : {}".format(self.queue_size))
 
     def unsubscribe(self):
         for sub in self.subs:
             sub.sub.unregister()
 
     def draw_rects_callback(self, img_msg, rects_msg, class_msg=None):
-        bridge = cv_bridge.CvBridge()
-        cv_img = bridge.imgmsg_to_cv2(img_msg, desired_encoding='bgr8')
+        start_time = rospy.Time.now()
+        if self.transport_hint == 'compressed':
+            # decode compressed image
+            np_arr = np.fromstring(img_msg.data, np.uint8)
+            cv_img = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
+            if img_msg.format.find("compressed rgb") > -1:
+                cv_img = cv_img[:, :, ::-1]
+        else:
+            cv_img = self.bridge.imgmsg_to_cv2(img_msg, desired_encoding='bgr8')
 
         img = cv2.resize(cv_img, None,
                          fx=self.resolution_factor,
@@ -130,10 +161,19 @@ def draw_rects_callback(self, img_msg, rects_msg, class_msg=None):
                     color=(255, 255, 255),
                     background_color=tuple(color),
                     offset_x=self.rect_boldness / 2.0)
-        viz_msg = bridge.cv2_to_imgmsg(img, encoding='bgr8')
+        if self.transport_hint == 'compressed':
+            viz_msg = sensor_msgs.msg.CompressedImage()
+            viz_msg.format = "jpeg"
+            viz_msg.data = np.array(cv2.imencode('.jpg', img)[1]).tostring()
+        else:
+            viz_msg = self.bridge.cv2_to_imgmsg(img, encoding='bgr8')
         viz_msg.header = img_msg.header
         self.pub_viz.publish(viz_msg)
 
+        rospy.loginfo("processing time {} on message taken at {} sec ago".format(
+            (rospy.Time.now() - start_time).to_sec(),
+            (rospy.Time.now() - img_msg.header.stamp).to_sec()))
+
 
 if __name__ == '__main__':
     rospy.init_node('draw_rects')

jsk_perception/sample/sample_aws_auto_checkin_app.launch

@@ -2,6 +2,7 @@
   <arg name="gui" default="true" doc="set true to show windows displaying face detection and face search"/>
   <arg name="use_usb_cam" default="false" doc="set true to use USB camera image as input"/>
   <arg name="use_aws_face_detection" default="false" doc="set true to use aws face detection API"/>
+  <arg name="always_publish" default="true" doc="set false not to publish result when face is not detected" />
 
   <node name="usb_cam" pkg="usb_cam" type="usb_cam_node"
         if="$(arg use_usb_cam)" >
@@ -23,6 +24,7 @@
       <rosparam subst_value="true">
         use_window: $(arg gui)
         aws_credentials_path: /tmp/aws.json
+        always_publish: $(arg always_publish)
         attributes: ALL
       </rosparam>
     </node>
@@ -45,8 +47,9 @@
       use_window: $(arg gui)
       env_path: /tmp/env.json
       aws_credentials_path: /tmp/aws.json
-      approximate_sync: false
-      queue_size: 100
+      always_publish: $(arg always_publish)
+      approximate_sync: true
+      queue_size: 1
       image_transport: compressed
     </rosparam>
   </node>
@@ -63,14 +66,17 @@
         font_path: $(find jsk_recognition_utils)/font_data/NotoSansJP-Regular.otf
         use_classification_result: true
         label_size: 16
-        queue_size: 100
+        approximate_sync: true
+        queue_size: 30
+        image_transport: compressed
       </rosparam>
     </node>
   </group>
 
   <group if="$(arg gui)" >
     <node name="image_view00"
           pkg="image_view" type="image_view">
+      <param name="image_transport" value="compressed" />
       <remap from="image" to="aws_auto_checkin_app/draw_rects/output" />
     </node>
   </group>