def _apply(self, img_msg, depth_msg): # validation if depth_msg.encoding == '16UC1': jsk_logwarn('Unsupported depth image encoding: {0}'.format( depth_msg.encoding)) if not (img_msg.height == depth_msg.height and img_msg.width == depth_msg.width): return # split fg/bg and get each mask bridge = cv_bridge.CvBridge() depth = bridge.imgmsg_to_cv2(depth_msg) if depth_msg.encoding == '32FC1': # convert float to int (handle 32FC1 as 16UC1) depth = depth.copy() depth[np.isnan(depth)] = 0 depth *= 255 depth = depth.astype(np.uint8) fg_mask, bg_mask = split_fore_background(depth) # publish cropped img = bridge.imgmsg_to_cv2(img_msg, desired_encoding='bgr8') fg = img.copy() fg[~fg_mask] = 0 bg = img.copy() bg[~bg_mask] = 0 self.fg_pub_.publish( bridge.cv2_to_imgmsg(fg, encoding=img_msg.encoding)) self.bg_pub_.publish( bridge.cv2_to_imgmsg(bg, encoding=img_msg.encoding)) fg_mask = (fg_mask * 255).astype(np.uint8) bg_mask = (bg_mask * 255).astype(np.uint8) self.fg_mask_pub_.publish( bridge.cv2_to_imgmsg(fg_mask, encoding='mono8')) self.bg_mask_pub_.publish( bridge.cv2_to_imgmsg(bg_mask, encoding='mono8'))
def _apply(self, depth_msg): # validation supported_encodings = {'16UC1', '32FC1'} if depth_msg.encoding not in supported_encodings: jsk_logwarn('Unsupported depth image encoding: {0}' .format(depth_msg.encoding)) # split fg/bg and get each mask bridge = cv_bridge.CvBridge() depth = bridge.imgmsg_to_cv2(depth_msg) nan_mask = None if depth_msg.encoding == '32FC1': # convert float to int (handle 32FC1 as 16UC1) nan_mask = np.isnan(depth) depth[nan_mask] = 0 depth *= 255 depth = depth.astype(np.uint8) fg_mask, bg_mask = split_fore_background(depth) if nan_mask is not None: fg_mask[nan_mask] = 0 bg_mask[nan_mask] = 0 # fg_mask fg_mask = (fg_mask * 255).astype(np.uint8) fg_mask_msg = bridge.cv2_to_imgmsg(fg_mask, encoding='mono8') fg_mask_msg.header = depth_msg.header self.fg_mask_pub_.publish(fg_mask_msg) # bg_mask bg_mask = (bg_mask * 255).astype(np.uint8) bg_mask_msg = bridge.cv2_to_imgmsg(bg_mask, encoding='mono8') bg_mask_msg.header = depth_msg.header self.bg_mask_pub_.publish(bg_mask_msg)
def _apply(self, img_msg, depth_msg): # validation if depth_msg.encoding == '16UC1': jsk_logwarn('Unsupported depth image encoding: {0}' .format(depth_msg.encoding)) if not (img_msg.height == depth_msg.height and img_msg.width == depth_msg.width): return # split fg/bg and get each mask bridge = cv_bridge.CvBridge() depth = bridge.imgmsg_to_cv2(depth_msg) if depth_msg.encoding == '32FC1': # convert float to int (handle 32FC1 as 16UC1) depth = depth.copy() depth[np.isnan(depth)] = 0 depth *= 255 depth = depth.astype(np.uint8) fg_mask, bg_mask = split_fore_background(depth) # publish cropped img = bridge.imgmsg_to_cv2(img_msg, desired_encoding='bgr8') fg = img.copy() fg[~fg_mask] = 0 bg = img.copy() bg[~bg_mask] = 0 self.fg_pub_.publish( bridge.cv2_to_imgmsg(fg, encoding=img_msg.encoding)) self.bg_pub_.publish( bridge.cv2_to_imgmsg(bg, encoding=img_msg.encoding)) fg_mask = (fg_mask * 255).astype(np.uint8) bg_mask = (bg_mask * 255).astype(np.uint8) self.fg_mask_pub_.publish( bridge.cv2_to_imgmsg(fg_mask, encoding='mono8')) self.bg_mask_pub_.publish( bridge.cv2_to_imgmsg(bg_mask, encoding='mono8'))
def __init__(self): rospy.init_node('delay_timestamp') jsk_logwarn('This node is mainly designed for TEST. ' 'Take care if you use this in your actual system.') self._master = rosgraph.Master(rospy.get_name()) self._delay = rospy.get_param('~delay') self._sub = rospy.Subscriber('~input', AnyMsg, self._cb) self._unknown_topic_type = True
def _cb_dyn_reconfig(self, config, level): file_name = config['file_name'] config['file_name'] = os.path.abspath(file_name) img_bgr = cv2.imread(file_name) if img_bgr is None: jsk_logwarn('Could not read image file: {}'.format(file_name)) with self.lock: self.imgmsg = None else: jsk_loginfo('Read the image file: {}'.format(file_name)) with self.lock: self.imgmsg = self.cv2_to_imgmsg(img_bgr, self.encoding) return config
def publish(self): now = rospy.Time.now() bridge = cv_bridge.CvBridge() img_bgr = cv2.imread(self.file_name) if img_bgr is None: jsk_logwarn('cannot read the image at {}' .format(self.file_name)) return # resolve encoding encoding = rospy.get_param('~encoding', 'bgr8') if getCvType(encoding) == 0: # mono8 img = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY) elif getCvType(encoding) == 2: # 16UC1 img = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY) img = img.astype(np.float32) img = img / 255 * (2 ** 16) img = img.astype(np.uint16) elif getCvType(encoding) == 5: # 32FC1 img = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY) img = img.astype(np.float32) img /= 255 elif getCvType(encoding) == 16: # 8UC3 if encoding in ('rgb8', 'rgb16'): # RGB img = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2RGB) else: img = img_bgr else: jsk_logerr('unsupported encoding: {0}'.format(encoding)) return # setup ros message and publish imgmsg = bridge.cv2_to_imgmsg(img, encoding=encoding) imgmsg.header.stamp = now imgmsg.header.frame_id = self.frame_id self.pub.publish(imgmsg) if self.publish_info: info = CameraInfo() info.header.stamp = now info.header.frame_id = self.frame_id info.width = imgmsg.width info.height = imgmsg.height self.pub_info.publish(info)
def publish(self): now = rospy.Time.now() bridge = cv_bridge.CvBridge() img_bgr = cv2.imread(self.file_name) if img_bgr is None: jsk_logwarn('cannot read the image at {}'.format(self.file_name)) return # resolve encoding encoding = rospy.get_param('~encoding', 'bgr8') if getCvType(encoding) == 0: # mono8 img = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY) elif getCvType(encoding) == 2: # 16UC1 img = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY) img = img.astype(np.float32) img = img / 255 * (2**16) img = img.astype(np.uint16) elif getCvType(encoding) == 5: # 32FC1 img = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2GRAY) img = img.astype(np.float32) img /= 255 elif getCvType(encoding) == 16: # 8UC3 if encoding in ('rgb8', 'rgb16'): # RGB img = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2RGB) else: img = img_bgr else: jsk_logerr('unsupported encoding: {0}'.format(encoding)) return # setup ros message and publish imgmsg = bridge.cv2_to_imgmsg(img, encoding=encoding) imgmsg.header.stamp = now imgmsg.header.frame_id = self.frame_id self.pub.publish(imgmsg) if self.publish_info: info = CameraInfo() info.header.stamp = now info.header.frame_id = self.frame_id info.width = imgmsg.width info.height = imgmsg.height self.pub_info.publish(info)