def _callback(self, camera_info):
if self.shelf == {}:
return
target_bin_name = rospy.get_param('~target_bin_name')
if target_bin_name not in 'abcdefghijkl':
rospy.logwarn('wrong target_bin_name')
return
if target_bin_name == '':
log_utils.logwarn_throttle(10, 'target_bin_name is empty string. This shows up every 10 seconds.')
return
target_bin = self.shelf[target_bin_name]
self.camera_model.fromCameraInfo(camera_info)
# get transform
camera2bb_base = self.tf_buffer.lookup_transform(
target_frame=camera_info.header.frame_id,
source_frame=target_bin.bbox.header.frame_id,
time=rospy.Time(0),
timeout=rospy.Duration(10.0))
mask_img = self.get_mask_img(camera2bb_base, target_bin, self.camera_model)
if np.all(mask_img == 0):
log_utils.logwarn_throttle(10, 'Bin mask image is all zero. ' +
'Position of an arm might be wrong.')
return
mask_msg = self.bridge.cv2_to_imgmsg(mask_img, encoding="mono8")
mask_msg.header = camera_info.header
self.pub.publish(mask_msg)
def __init__(self):
super(self.__class__, self).__init__()
# set target_names
self.target_names = ['background'] + \
[datum['name']
for datum in jsk_apc2016_common.get_object_data()]
n_class = len(self.target_names)
assert n_class == 40
# load model
self.gpu = rospy.get_param('~gpu', 0)
chainermodel = rospy.get_param('~chainermodel')
self.model = FCN32s(n_class=n_class)
S.load_hdf5(chainermodel, self.model)
if self.gpu != -1:
self.model.to_gpu(self.gpu)
jsk_logwarn('>> Model is loaded <<')
while True:
self.tote_contents = rospy.get_param('~tote_contents', None)
if self.tote_contents is not None:
break
logwarn_throttle(10, 'param ~tote_contents is not set. Waiting..')
rospy.sleep(0.1)
self.label_names = rospy.get_param('~label_names')
jsk_logwarn('>> Param is set <<')
self.pub = self.advertise('~output', Image, queue_size=1)
self.pub_debug = self.advertise('~debug', Image, queue_size=1)
def __init__(self):
super(self.__class__, self).__init__()
# set target_names
self.target_names = ['background'] + \
[datum['name']
for datum in jsk_apc2016_common.get_object_data()]
n_class = len(self.target_names)
assert n_class == 40
# load model
self.gpu = rospy.get_param('~gpu', 0)
chainermodel = rospy.get_param('~chainermodel')
self.model = FCN32s(n_class=n_class)
S.load_hdf5(chainermodel, self.model)
if self.gpu != -1:
self.model.to_gpu(self.gpu)
jsk_logwarn('>> Model is loaded <<')
while True:
self.tote_contents = rospy.get_param('~tote_contents', None)
if self.tote_contents is not None:
break
logwarn_throttle(10, 'param ~tote_contents is not set. Waiting..')
rospy.sleep(0.1)
self.label_names = rospy.get_param('~label_names')
jsk_logwarn('>> Param is set <<')
self.pub = self.advertise('~output', Image, queue_size=1)
self.pub_debug = self.advertise('~debug', Image, queue_size=1)
def _callback(self, camera_info):
if self.shelf == {}:
return
target_bin_name = rospy.get_param('~target_bin_name', '')
if target_bin_name not in 'abcdefghijkl':
rospy.logwarn('wrong target_bin_name')
return
if target_bin_name == '':
log_utils.logwarn_throttle(10, 'target_bin_name is empty string. This shows up every 10 seconds.')
return
target_bin = self.shelf[target_bin_name]
self.camera_model.fromCameraInfo(camera_info)
# get transform
camera2bb_base = self.tf_buffer.lookup_transform(
target_frame=camera_info.header.frame_id,
source_frame=target_bin.bbox.header.frame_id,
time=rospy.Time(0),
timeout=rospy.Duration(10.0))
mask_img = self.get_mask_img(camera2bb_base, target_bin, self.camera_model)
if np.all(mask_img == 0):
log_utils.logwarn_throttle(10, 'Bin mask image is all zero. ' +
'Position of an arm might be wrong.')
return
mask_msg = self.bridge.cv2_to_imgmsg(mask_img, encoding="mono8")
mask_msg.header = camera_info.header
self.pub.publish(mask_msg)
def _publish(self, event):
if self.bin_info_dict == {}:
return
target_bin_name = rospy.get_param('~target_bin_name', '')
if target_bin_name not in 'abcdefghijkl':
rospy.logwarn('wrong target_bin_name')
return
if target_bin_name == '':
log_utils.logwarn_throttle(10, 'target_bin_name is empty string. This shows up every 10 seconds.')
return
with self.lock:
self.bin_info_dict[target_bin_name].header.seq = (
self.bin_info_dict[target_bin_name].header.seq + 1)
target_bin_info = self.bin_info_dict[target_bin_name]
target_bin_info.header.stamp = rospy.Time.now()
self.target_bin_info_pub.publish(target_bin_info)
def _callback(self, imgmsg):
if self.channel < 0:
return
bridge = CvBridgeArbitraryChannels()
img = bridge.imgmsg_to_cv2(imgmsg)
if img.ndim == 2:
img = img[:, :, np.newaxis]
if not (self.channel < img.shape[2]):
logwarn_throttle(10,
'Invalid channel {} is specified for image with {} channels'
.format(self.channel, img.shape[2]))
return
out_img = np.zeros(img.shape[:2], dtype=img.dtype)
out_img = img[:, :, self.channel]
out_imgmsg = bridge.cv2_to_imgmsg(out_img)
out_imgmsg.header = imgmsg.header
self.pub.publish(out_imgmsg)
def _publish(self, event):
if self.bin_info_dict == {}:
return
target_bin_name = rospy.get_param('~target_bin_name', '')
if target_bin_name not in 'abcdefghijkl':
rospy.logwarn('wrong target_bin_name')
return
if target_bin_name == '':
log_utils.logwarn_throttle(
10,
'target_bin_name is empty string. This shows up every 10 seconds.'
)
return
with self.lock:
self.bin_info_dict[target_bin_name].header.seq = (
self.bin_info_dict[target_bin_name].header.seq + 1)
target_bin_info = self.bin_info_dict[target_bin_name]
target_bin_info.header.stamp = rospy.Time.now()
self.target_bin_info_pub.publish(target_bin_info)
def _callback(self, img_msg, mask_msg):
bridge = cv_bridge.CvBridge()
bgr_img = bridge.imgmsg_to_cv2(img_msg, desired_encoding='bgr8')
mask_img = bridge.imgmsg_to_cv2(mask_msg, desired_encoding='mono8')
if mask_img.size < 1:
logwarn_throttle(10, 'Too small sized image')
return
logwarn_throttle(10, '[FCNMaskForLabelNames] >> Start Processing <<')
if mask_img.ndim == 3 and mask_img.shape[2] == 1:
mask_img = mask_img.reshape(mask_img.shape[:2])
if mask_img.shape != bgr_img.shape[:2]:
jsk_logwarn('Size of mask and color image is different.'
'Resizing.. mask {0} to {1}'
.format(mask_img.shape, bgr_img.shape[:2]))
mask_img = resize(mask_img, bgr_img.shape[:2],
preserve_range=True).astype(np.uint8)
blob = bgr_img - self.mean_bgr
blob = blob.transpose((2, 0, 1))
x_data = np.array([blob], dtype=np.float32)
if self.gpu != -1:
x_data = cuda.to_gpu(x_data, device=self.gpu)
x = Variable(x_data, volatile=True)
self.model(x)
pred_datum = cuda.to_cpu(self.model.score.data[0])
candidate_labels = [self.target_names.index(name)
for name in self.tote_contents]
label_pred_in_candidates = pred_datum[candidate_labels].argmax(axis=0)
label_pred = np.zeros_like(label_pred_in_candidates)
for idx, label_val in enumerate(candidate_labels):
label_pred[label_pred_in_candidates == idx] = label_val
label_pred[mask_img == 0] = 0 # set bg_label
label_viz = label2rgb(label_pred, bgr_img, bg_label=0)
label_viz = (label_viz * 255).astype(np.uint8)
debug_msg = bridge.cv2_to_imgmsg(label_viz, encoding='rgb8')
debug_msg.header = img_msg.header
self.pub_debug.publish(debug_msg)
output_mask = np.ones(mask_img.shape, dtype=np.uint8)
output_mask *= 255
for label_val, label_name in enumerate(self.target_names):
if label_name in self.label_names:
assert label_name == 'kleenex_paper_towels'
assert label_val == 21
label_mask = ((label_pred == label_val) * 255).astype(np.uint8)
contours, hierachy = cv2.findContours(
label_mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
cv2.drawContours(output_mask, contours, -1, 255, -1)
# output_mask[label_pred == label_val] = False
# output_mask = output_mask.astype(np.uint8)
# output_mask[output_mask == 1] = 255
output_mask[mask_img == 0] = 0
output_mask_msg = bridge.cv2_to_imgmsg(output_mask, encoding='mono8')
output_mask_msg.header = img_msg.header
self.pub.publish(output_mask_msg)
logwarn_throttle(10, '[FCNMaskForLabelNames] >> Finshed processing <<')
def _callback(self, img_msg, mask_msg):
bridge = cv_bridge.CvBridge()
bgr_img = bridge.imgmsg_to_cv2(img_msg, desired_encoding='bgr8')
mask_img = bridge.imgmsg_to_cv2(mask_msg, desired_encoding='mono8')
if mask_img.size < 1:
logwarn_throttle(10, 'Too small sized image')
return
logwarn_throttle(10, '[FCNMaskForLabelNames] >> Start Processing <<')
if mask_img.ndim == 3 and mask_img.shape[2] == 1:
mask_img = mask_img.reshape(mask_img.shape[:2])
if mask_img.shape != bgr_img.shape[:2]:
jsk_logwarn('Size of mask and color image is different.'
'Resizing.. mask {0} to {1}'.format(
mask_img.shape, bgr_img.shape[:2]))
mask_img = resize(mask_img, bgr_img.shape[:2],
preserve_range=True).astype(np.uint8)
blob = bgr_img - self.mean_bgr
blob = blob.transpose((2, 0, 1))
x_data = np.array([blob], dtype=np.float32)
if self.gpu != -1:
x_data = cuda.to_gpu(x_data, device=self.gpu)
x = Variable(x_data, volatile=True)
self.model(x)
pred_datum = cuda.to_cpu(self.model.score.data[0])
candidate_labels = [
self.target_names.index(name) for name in self.tote_contents
]
label_pred_in_candidates = pred_datum[candidate_labels].argmax(axis=0)
label_pred = np.zeros_like(label_pred_in_candidates)
for idx, label_val in enumerate(candidate_labels):
label_pred[label_pred_in_candidates == idx] = label_val
label_pred[mask_img == 0] = 0 # set bg_label
label_viz = label2rgb(label_pred, bgr_img, bg_label=0)
label_viz = (label_viz * 255).astype(np.uint8)
debug_msg = bridge.cv2_to_imgmsg(label_viz, encoding='rgb8')
debug_msg.header = img_msg.header
self.pub_debug.publish(debug_msg)
output_mask = np.ones(mask_img.shape, dtype=np.uint8)
output_mask *= 255
for label_val, label_name in enumerate(self.target_names):
if label_name in self.label_names:
assert label_name == 'kleenex_paper_towels'
assert label_val == 21
label_mask = ((label_pred == label_val) * 255).astype(np.uint8)
contours, hierachy = cv2.findContours(label_mask,
cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)
cv2.drawContours(output_mask, contours, -1, 255, -1)
# output_mask[label_pred == label_val] = False
# output_mask = output_mask.astype(np.uint8)
# output_mask[output_mask == 1] = 255
output_mask[mask_img == 0] = 0
output_mask_msg = bridge.cv2_to_imgmsg(output_mask, encoding='mono8')
output_mask_msg.header = img_msg.header
self.pub.publish(output_mask_msg)
logwarn_throttle(10, '[FCNMaskForLabelNames] >> Finshed processing <<')
