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 get_sorted_work_order(self):
"""Sort work order to maximize the score"""
bin_order = self.work_order.keys()
if self.object_data is not None:
if all(self.gripper in x for x in
[d['graspability'].keys() for d in self.object_data]):
def get_graspability(bin_):
target_object = self.work_order[bin_]
target_object_data = [
data for data in self.object_data
if data['name'] == target_object
][0]
graspability = target_object_data['graspability'][
self.gripper]
return graspability
# re-sort the work_order with graspability
bin_order = sorted(bin_order, key=get_graspability)
else:
jsk_logwarn('Not sorted by graspability')
jsk_logwarn(
'Not all object_data have graspability key: {gripper}'.
format(gripper=self.gripper))
bin_order = sorted(self.work_order.keys(),
key=lambda bin_: len(self.bin_contents[bin_]))
work_order = [(bin_, self.work_order[bin_]) for bin_ in bin_order]
return work_order
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 get_work_order_msg(self):
work_order = self.get_sorted_work_order()
if self.max_weight == -1:
self.max_weight = np.inf
msg = dict(left=WorkOrderArray(), right=WorkOrderArray())
abandon_target_objects = [
'genuine_joe_plastic_stir_sticks',
'cheezit_big_original',
'rolodex_jumbo_pencil_cup',
'champion_copper_plus_spark_plug',
'oreo_mega_stuf',
]
abandon_bin_objects = [
'rolodex_jumbo_pencil_cup',
'oreo_mega_stuf'
]
# >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
# TODO: PLEASE FILL ABANDON BINS
abandon_bins = ''
# <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
for bin_, target_object in work_order:
if bin_ in abandon_bins:
jsk_logwarn('Skipping bin {bin} because of user request'
.format(bin=bin_))
continue
if self.object_data is not None:
target_object_data = [data for data in self.object_data
if data['name'] == target_object][0]
if target_object_data['weight'] > self.max_weight:
jsk_logwarn('Skipping target {obj} in {bin_}: it exceeds max weight {weight} > {max_weight}'
.format(obj=target_object_data['name'], bin_=bin_,
weight=target_object_data['weight'], max_weight=self.max_weight))
continue
if target_object_data['graspability'][self.gripper] > 3:
jsk_logwarn('Skipping target {obj} in {bin_}: it exceeds graspability'
'{grasp} > {max_grasp} with gripper {gripper}'
.format(obj=target_object_data['name'], bin_=bin_,
grasp=target_object_data['graspability'][self.gripper],
max_grasp=3,
gripper=self.gripper))
continue
else:
if target_object in abandon_target_objects:
jsk_logwarn('Skipping target {obj} in {bin_}: it is listed as abandon target'
.format(obj=target_object, bin_=bin_))
continue
if any(bin_object in abandon_bin_objects for bin_object in self.bin_contents[bin_]):
jsk_logwarn('Skipping {bin_}: this bin contains abandon objects'.format(bin_=bin_))
continue
# if len(bin_contents[bin_]) > 5: # Level3
# jsk_logwarn('Skipping {bin_}: this bin contains more than 5 objects'.format(bin_=bin_))
# continue
order = WorkOrder(bin=bin_, object=target_object)
if bin_ in 'abdegj':
msg['left'].array.append(order)
elif bin_ in 'cfhikl':
msg['right'].array.append(order)
return msg
def __init__(self):
rp = rospkg.RosPack()
input_path = rospy.get_param('~json', None)
if input_path is None:
jsk_logwarn('must set json file path to param ~json')
return
with open(input_path, 'r') as f:
self.output_data = json.load(f)
self.tote_contents = self.output_data['tote_contents']
self.bin_contents = self.output_data['bin_contents']
self.work_order = jsk_apc2016_common.get_work_order(input_path)
self.output_path = osp.join(
rp.get_path('jsk_2016_01_baxter_apc'),
'output',
'output_' + osp.basename(input_path))
self.arm_state = {}
self.arm_target_bin = {}
self.finished_bin_list = []
with open(self.output_path, 'w') as f:
json.dump(self.output_data, f, sort_keys=True, indent=4)
def get_sorted_work_order(self):
"""Sort work order to maximize the score"""
bin_order = self.work_order.keys()
if self.object_data is not None:
if all(self.gripper in x for x in [d['graspability'].keys() for d in self.object_data]):
def get_graspability(bin_):
target_object = self.work_order[bin_]
target_object_data = [data for data in self.object_data
if data['name'] == target_object][0]
graspability = target_object_data['graspability'][self.gripper]
return graspability
# re-sort the work_order with graspability
bin_order = sorted(bin_order, key=get_graspability)
else:
jsk_logwarn('Not sorted by graspability')
jsk_logwarn('Not all object_data have graspability key: {gripper}'
.format(gripper=self.gripper))
bin_order = sorted(self.work_order.keys(),
key=lambda bin_: len(self.bin_contents[bin_]))
work_order = [(bin_, self.work_order[bin_]) for bin_ in bin_order]
return work_order
def test_jsk_logwarn(self, mock_get_name, mock_log):
mock_get_name.return_value = '/spam'
jsk_logwarn('spam')
mock_log.assert_called_with('[/spam] [TestJSKLogXXX::test_jsk_logwarn] spam')
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 test_jsk_logwarn(self, mock_log):
jsk_logwarn('spam')
mock_log.assert_called_with('[TestJSKLogXXX::test_jsk_logwarn] spam')
def get_work_order_msg(self):
work_order = self.get_sorted_work_order()
if self.max_weight == -1:
self.max_weight = np.inf
msg = dict(left=WorkOrderArray(), right=WorkOrderArray())
abandon_target_objects = [
'genuine_joe_plastic_stir_sticks',
'cheezit_big_original',
'rolodex_jumbo_pencil_cup',
'champion_copper_plus_spark_plug',
'oreo_mega_stuf',
]
abandon_bin_objects = ['rolodex_jumbo_pencil_cup', 'oreo_mega_stuf']
# >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
# TODO: PLEASE FILL ABANDON BINS
abandon_bins = ''
# <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
for bin_, target_object in work_order:
if bin_ in abandon_bins:
jsk_logwarn(
'Skipping bin {bin} because of user request'.format(
bin=bin_))
continue
if self.object_data is not None:
target_object_data = [
data for data in self.object_data
if data['name'] == target_object
][0]
if target_object_data['weight'] > self.max_weight:
jsk_logwarn(
'Skipping target {obj} in {bin_}: it exceeds max weight {weight} > {max_weight}'
.format(obj=target_object_data['name'],
bin_=bin_,
weight=target_object_data['weight'],
max_weight=self.max_weight))
continue
if target_object_data['graspability'][self.gripper] > 3:
jsk_logwarn(
'Skipping target {obj} in {bin_}: it exceeds graspability'
'{grasp} > {max_grasp} with gripper {gripper}'.format(
obj=target_object_data['name'],
bin_=bin_,
grasp=target_object_data['graspability'][
self.gripper],
max_grasp=3,
gripper=self.gripper))
continue
else:
if target_object in abandon_target_objects:
jsk_logwarn(
'Skipping target {obj} in {bin_}: it is listed as abandon target'
.format(obj=target_object, bin_=bin_))
continue
if any(bin_object in abandon_bin_objects
for bin_object in self.bin_contents[bin_]):
jsk_logwarn(
'Skipping {bin_}: this bin contains abandon objects'.
format(bin_=bin_))
continue
# if len(bin_contents[bin_]) > 5: # Level3
# jsk_logwarn('Skipping {bin_}: this bin contains more than 5 objects'.format(bin_=bin_))
# continue
order = WorkOrder(bin=bin_, object=target_object)
if bin_ in 'abdegj':
msg['left'].array.append(order)
elif bin_ in 'cfhikl':
msg['right'].array.append(order)
return msg
