def cv2_to_imgmsg(self, img_bgr, encoding):
     bridge = cv_bridge.CvBridge()
     # resolve encoding
     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
     return bridge.cv2_to_imgmsg(img, encoding=encoding)
示例#2
0
 def cv2_to_imgmsg(self, img_bgr, encoding):
     bridge = cv_bridge.CvBridge()
     # resolve encoding
     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
     return bridge.cv2_to_imgmsg(img, encoding=encoding)
示例#3
0
 def __init__(self, from_topic):
     self.pub = rospy.Publisher('/robot/xdisplay', Image, queue_size=10)
     self.sub = rospy.Subscriber(from_topic, Image, self.cb)
     self.do_resize = rospy.get_param('~resize', True)
     self.do_centerize = rospy.get_param('~centerize', True)
     if not self.do_resize and self.do_centerize:
         jsk_topic_tools.jsk_logerr(
             "If '~centerize' is True, '~resize' must be True also."
             " Stopping centerizing.")
         self.do_centerize = False
 def __init__(self, from_topic):
     self.pub = rospy.Publisher('/robot/xdisplay', Image, queue_size=10)
     self.sub = rospy.Subscriber(from_topic, Image, self.cb)
     self.do_resize = rospy.get_param('~resize', True)
     self.do_centerize = rospy.get_param('~centerize', True)
     if not self.do_resize and self.do_centerize:
         jsk_topic_tools.jsk_logerr(
             "If '~centerize' is True, '~resize' must be True also."
             " Stopping centerizing.")
         self.do_centerize = False
 def __init__(self, from_topic):
     self.pub = rospy.Publisher('/robot/xdisplay', Image, queue_size=10)
     self.sub = rospy.Subscriber(from_topic, Image, self.cb)
     self.do_resize = rospy.get_param('~resize', True)
     self.do_centerize = rospy.get_param('~centerize', True)
     # JSK baxter's xdisplay is 1920x1200.
     # But default baxter's xdisplay is 1024x600.
     self.max_width = rospy.get_param('~max_width', 1920)
     self.max_height = rospy.get_param('~max_height', 1200)
     if not self.do_resize and self.do_centerize:
         jsk_topic_tools.jsk_logerr(
             "If '~centerize' is True, '~resize' must be True also."
             " Stopping centerizing.")
         self.do_centerize = False
示例#6
0
 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)
 def _apply(self, img_msg, 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))
     if not (img_msg.height == depth_msg.height and
             img_msg.width == depth_msg.width):
         jsk_logerr('size of raw and depth image is different')
         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
     fg = img.copy()
     fg[~fg_mask] = 0
     fg_msg = bridge.cv2_to_imgmsg(fg, encoding=img_msg.encoding)
     fg_msg.header = img_msg.header
     self.fg_pub_.publish(fg_msg)
     # bg
     bg = img.copy()
     bg[~bg_mask] = 0
     bg_msg = bridge.cv2_to_imgmsg(bg, encoding=img_msg.encoding)
     bg_msg.header = img_msg.header
     self.bg_pub_.publish(bg_msg)
     # 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
     supported_encodings = {'16UC1', '32FC1'}
     if depth_msg.encoding not in supported_encodings:
         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):
         jsk_logerr('size of raw and depth image is different')
         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
     fg = img.copy()
     fg[~fg_mask] = 0
     fg_msg = bridge.cv2_to_imgmsg(fg, encoding=img_msg.encoding)
     fg_msg.header = img_msg.header
     self.fg_pub_.publish(fg_msg)
     # bg
     bg = img.copy()
     bg[~bg_mask] = 0
     bg_msg = bridge.cv2_to_imgmsg(bg, encoding=img_msg.encoding)
     bg_msg.header = img_msg.header
     self.bg_pub_.publish(bg_msg)
     # 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)