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