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)
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)
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
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)